size_t HardwareSerial::write(uint8_t c)
{
  int i = (_tx_buffer->head + 1) % SERIAL_BUFFER_SIZE;
	
  // If the output buffer is full, there's nothing for it other than to 
  // wait for the interrupt handler to empty it a bit
  // ???: return 0 here instead?
  while (i == _tx_buffer->tail)
    ;
	
  _tx_buffer->buffer[_tx_buffer->head] = c;
  _tx_buffer->head = i;
	
  sbi(*_ucsrb, _udrie);
  
  return 1;
}

size_t HardwareSerial::write9(uint16_t c, bool cmd)
{
  int i = (_tx_buffer->head + 1) % SERIAL_BUFFER_SIZE;
	
  // If the output buffer is full, there's nothing for it other than to 
  // wait for the interrupt handler to empty it a bit
  // ???: return 0 here instead?
  while (i == _tx_buffer->tail)
    ;
  //we add the leading 1 to be sure
  _tx_buffer->buffer[_tx_buffer->head] = (cmd ? c|0x100 : c);
  _tx_buffer->head = i;
	
  sbi(*_ucsrb, _udrie);
  
  return 1;  
}

size_t BiscuitSerial::write(uint8_t c)
{
  int i = (tx_buffer.head + 1) % SERIAL_BUFFER_SIZE;
	
  // If the output buffer is full, there's nothing for it other than to 
  // wait for the interrupt handler to empty it a bit
  // ???: return 0 here instead?
  while (i == tx_buffer.tail)
    ;
	
  tx_buffer.buffer[tx_buffer.head] = c;
  tx_buffer.head = i;
	
  sbi(UCSR0B, UDRIE0);
  
  return 1;
}

/* ADC_INIT: initialize the A/D to access the photo sensor */
char TOS_COMMAND(ADC_INIT)(){
#ifndef FULLPC
    sbi(DDRB, 4);
    sbi(PORTB, 4);
    sbi(DDRB, 3);
    sbi(PORTB, 3);
    sbi(DDRD, 5);
    sbi(PORTD, 5);
    outp(0x07, ADCSR);
	
    cbi(ADCSR, ADSC);
    sbi(ADCSR, ADIE);
    sbi(ADCSR, ADEN);
#else 
    printf("ADC initialized.\n");

#endif
    return 0;
}

uint8_t i2cMasterSendNI(uint8_t deviceAddr, uint8_t length, uint8_t const *data)
{
    uint8_t retval = I2C_OK;

    // disable TWI interrupt
    cbi(TWCR, TWIE);

    // send start condition
    i2cSendStart();
    i2cWaitForComplete();

    // send device address with write
    i2cSendByte(deviceAddr & 0xFE);
    i2cWaitForComplete();

    // check if device is present and live
    if (inb(TWSR) == TW_MT_SLA_ACK)
    {
        // send data
        while (length)
        {
            i2cSendByte(*data++);
            i2cWaitForComplete();
            length--;
        }
    }
    else
    {
        // device did not ACK it's address,
        // data will not be transferred
        // return error
        retval = I2C_ERROR_NODEV;
    }

    // transmit stop condition
    // leave with TWEA on for slave receiving
    i2cSendStop();
    while (!(inb(TWCR) & BV(TWSTO)))
        ;

    // enable TWI interrupt
    sbi(TWCR, TWIE);

    return retval;
}

int analogRead(uint8_t pin)
{
	uint8_t low, high;

	// set the analog reference (high two bits of ADMUX) and select the
	// channel (low 4 bits).  this also sets ADLAR (left-adjust result)
	// to 0 (the default).
//	ADMUX = (analog_reference << 6) | (pin & 0x3f); // more MUX
// sapo per tiny45
	//ADMUX = pin & 0x3f;

    // from tod
    // map arduino "pin" to ADC MUX value
    // from Table 20-3 in ATtiny45 datasheet
    if(        pin == PB5 ) {
        ADMUX = 0x00;
    } else if( pin == PB2 ) { 
        ADMUX = 0x01;
    } else if( pin == PB4 ) {
        ADMUX = 0x02;
    } else if( pin == PB3 ) {
        ADMUX = 0x03;
    } else {
        ADMUX = pin;  // in case people want to select temp sensor or whatever
    }
    
	// without a delay, we seem to read from the wrong channel
	//delay(1);

	// start the conversion
	sbi(ADCSRA, ADSC);

	// ADSC is cleared when the conversion finishes
	while (bit_is_set(ADCSRA, ADSC));

	// we have to read ADCL first; doing so locks both ADCL
	// and ADCH until ADCH is read.  reading ADCL second would
	// cause the results of each conversion to be discarded,
	// as ADCL and ADCH would be locked when it completed.
	low = ADCL;
	high = ADCH;

	// combine the two bytes
	return (high << 8) | low;
}

void net_receive()
{
	if (!com_check())
		return;

	com_connect();
	com_set_command(CMD_MA_REQ);
	com_config(BUS_INPUT);

	uint8_t uiLen =0;
	com_receive_chunk(&uiLen);
	com_disconnect();
	cDataReq = 0;
	sbi(GICR,INT0);

	uiLen = parser_parse();
	net_send(uiLen);
}

// read a single byte from address and return it as a byte
uint8_t i2cReadRegister(uint8_t i2c_7bit_address, uint8_t address)
{
  uint8_t data;

  i2cSendStart();
  i2cSendWriteAddress(i2c_7bit_address);
  i2cSendData(address);	// write register address
  i2cSendRepeatedStart();		// repeated start
  i2cSendReadAddress(i2c_7bit_address);
  i2cReceiveByte(0);
  data = i2cGetReceivedByte();	// Get result
  i2cSendStop();

  cbi(TWCR, TWEN);	// Disable TWI
  sbi(TWCR, TWEN);	// Enable TWI

  return data;
}

void SlaveRtu::init() {

	_de.set(LOW);
	_re.set(LOW);

#if defined(TCCR2A) && defined(TCCR2B)
	TIMER_CS(TCCR2B, 2, TIMER_WITHOUT_EXT_CLK_CS_128);
	TIMER_3BIT_WAVEFORM(2, TIMER_3BIT_WAVEFORM_CTC);
	// (11/19200) * 3.5 / (1/(16000000/128))
	OCR2A = 250;
	sbi(TIMSK2, OCIE2A);
#else
#error Timer 2 reg not found
#endif

	_usart.begin(19200, SERIAL_8E1);

}

// Right now, PWM output only works on the pins with
// hardware support.  These are defined in the appropriate
// pins_*.c file.  For the rest of the pins, we default
// to digital output.
void analogWrite(uint8_t pin, int val)
{
	// We need to make sure the PWM output is enabled for those pins
	// that support it, as we turn it off when digitally reading or
	// writing with them.  Also, make sure the pin is in output mode
	// for consistenty with Wiring, which doesn't require a pinMode
	// call for the analog output pins.
	pinMode(pin, OUTPUT);
	
	if (digitalPinToTimer(pin) == TIMER1A) {
		// connect pwm to pin on timer 1, channel A
		sbi(TCCR1A, COM1A1);
		// set pwm duty
		OCR1A = val;
	} else if (digitalPinToTimer(pin) == TIMER1B) {
		// connect pwm to pin on timer 1, channel B
		sbi(TCCR1A, COM1B1);
		// set pwm duty
		OCR1B = val;
	} else if (digitalPinToTimer(pin) == TIMER0A) {
		if (val == 0) {
			digitalWrite(pin, LOW);
		} else {
			// connect pwm to pin on timer 0, channel A
			sbi(TCCR0A, COM0A1);
			// set pwm duty
			OCR0A = val;      
		}
	} else if (digitalPinToTimer(pin) == TIMER0B) {
		if (val == 0) {
			digitalWrite(pin, LOW);
		} else {
			// connect pwm to pin on timer 0, channel B
			sbi(TCCR0A, COM0B1);
			// set pwm duty
			OCR0B = val;
		}
	} else if (digitalPinToTimer(pin) == TIMER2A) {
		// connect pwm to pin on timer 2, channel A
		sbi(TCCR2A, COM2A1);
		// set pwm duty
		OCR2A = val;	
	} else if (digitalPinToTimer(pin) == TIMER2B) {
		// connect pwm to pin on timer 2, channel B
		sbi(TCCR2A, COM2B1);
		// set pwm duty
		OCR2B = val;
	} else if (val < 128)
		digitalWrite(pin, LOW);
	else
		digitalWrite(pin, HIGH);
}

// functions
void i2cInit(void) {

#ifdef I2C_USE_INT_PULLUP_RESISTORS
	// set pull-up resistors on I2C bus pins
#if (defined (__AVR_ATmega64C1__) || defined (__AVR_ATmega64M1__) ||\
	defined (__AVR_ATmega128__) || defined (__AVR_ATmega1280__) ||\
	defined (__AVR_ATmega1281__) || defined (__AVR_ATmega1284P__) ||\
	defined (__AVR_ATmega128RFA1__) || defined(__AVR_ATmega2560__))

	sbi(PORTD, 0);	// i2c SCL on ATmega128,64
	sbi(PORTD, 1);	// i2c SDA on ATmega128,64
#elif (defined (__AVR_ATmega8__) || defined (__AVR_ATmega8A__))
    sbi(PORTC, 5); // i2c SCL on ATmega8
    sbi(PORTC, 4); // i2c SDA on ATmega8
#else
	sbi(PORTC, 0);	// i2c SCL on ATmega163,323,16,32,etc
	sbi(PORTC, 1);	// i2c SDA on ATmega163,323,16,32,etc
#endif
#endif

	// clear SlaveReceive and SlaveTransmit handler to null
	i2cSlaveReceive = 0;
	i2cSlaveTransmit = 0;
    i2cStopHandler = 0;
	// set i2c bit rate to 100KHz
	i2cSetBitrate(100);
	// enable TWI (two-wire interface)
	sbi(TWCR, TWEN);
	// set state
	I2cState = I2C_IDLE;
	// enable TWI interrupt and slave address ACK
	sbi(TWCR, TWIE);
	sbi(TWCR, TWEA);
	//outb(TWCR, (inb(TWCR)&TWCR_CMD_MASK)|BV(TWINT)|BV(TWEA));
	// enable interrupts
	sei();
}

int main() {    
    /* reset the ports, and set the directions */
    SET_PIN_DIRECTIONS();
    TOS_CALL_COMMAND(MAIN_SUB_POT_INIT)(0);
    TOS_sched_init();
    
    TOS_CALL_COMMAND(MAIN_SUB_INIT)();
    TOS_CALL_COMMAND(MAIN_SUB_START)();
    dbg(DBG_BOOT,("mote initialized.\n"));

    while(1){
	while(!TOS_schedule_task()) { };
	sbi(MCUCR, SE);
	asm volatile ("sleep" ::);
        asm volatile ("nop" ::);
        asm volatile ("nop" ::);
    }
}

void timer2A_enable(uint16_t interrupts_per_sec) {
    unsigned char sreg;
    sreg = SREG;
    cli();
    cbi(TIMSK2, OCIE2A);


    TCNT2=0;

    TCCR2A=(1<<WGM21);
    TCCR2B=(1<<CS21); // fclock/8, CTC mode


    TCNT2=0;
    OCR2A=(uint16_t)((uint32_t)(F_CPU/8)/(uint32_t)interrupts_per_sec);
    sbi(TIMSK2, OCIE2A);
    SREG = sreg;
}

//This init routine actually begins operation (as soon as interrupts get enabled)
//LED_ENCODE is OC2B
void init_irtx()
{
    //TIMER 2: 40kHz 50% Square Wave Generator
    cbi(PORTD, 3); //so that disabled OCR2B = 0 on output
    cbi(ASSR, 5); //clock from MCU mainline clock (16MHz)
    enable_output_irclock();
    TCCR2B = B8(00000001); //enable timer with no prescaler = 16MHz
    OCR2A = 200; //toggle after 12.5mS -> 25mS period = 40kHz freq (16MHz clock)
    TIMSK2 = B8(00000000); //no interrupts from this timer
    TIFR2; //Timer interrupt flag register
    sbi(DDRD, 3); //OCR2B as output

    //TIMER 1: Data encoding clock - pulse width determination
    TCCR1A = B8(00000000); //Normal counter
    TCCR1B = B8(00000100); //Divide by 256 (16MHz/256 = 62.5kHz)
    TCCR1C = B8(00000000); //No force compare matches

}

void uartswSendByte(u08 data)
{
	// wait until uart is ready
	while(UartswTxBusy);
	// set busy flag
	UartswTxBusy = TRUE;
	// save data
	UartswTxData = data;
	// set number of bits (+1 for stop bit)
	UartswTxBitNum = 9;
	
	// set the start bit
	cbi(UARTSW_TX_PORT, UARTSW_TX_PIN);//changed to cbi -JGM
	// schedule the next bit
	outb(OCR2, inb(TCNT2) + UartswBaudRateDiv); 
	// enable OC2 interrupt
	sbi(TIMSK, OCIE2);
}

// enable and initialize the software uart
void uartswInit(void)
{
	// initialize the buffers
	uartswInitBuffers();
	// initialize the ports
	sbi(UARTSW_TX_DDR, UARTSW_TX_PIN);
	#ifdef UARTSW_INVERT
	cbi(UARTSW_TX_PORT, UARTSW_TX_PIN);
	#else
	sbi(UARTSW_TX_PORT, UARTSW_TX_PIN);
	#endif
	cbi(UARTSW_RX_DDR, UARTSW_RX_PIN);
	#ifdef UARTSW_INVERT
	cbi(UARTSW_RX_PORT, UARTSW_RX_PIN);
	#else
	sbi(UARTSW_RX_PORT, UARTSW_RX_PIN);
	#endif
	// initialize baud rate
	uartswSetBaudRate(9600);

	// setup the transmitter
	UartswTxBusy = FALSE;
	// disable OC0A interrupt
	cbi(TIMSK0, OCIE0A);
	// attach TxBit service routine to OC0A
	timerAttach(TIMER0OUTCOMPAREA_INT, uartswTxBitService);

	// setup the receiver
	UartswRxBusy = FALSE;
	// disable 0C0B interrupt
	cbi(TIMSK0, OCIE0B);
	// attach RxBit service routine to OC0B
	timerAttach(TIMER0OUTCOMPAREB_INT, uartswRxBitService);
	// INT1 trigger on rising/falling edge
	#ifdef UARTSW_INVERT
	sbi(EICRA, ISC11);
	sbi(EICRA, ISC10);
	#else
	sbi(EICRA, ISC11);
	cbi(EICRA, ISC10);
	#endif
	// enable INT1 interrupt
	sbi(EIMSK, INT1);

	// turn on interrupts
	sei();
}

U08 TxPacket(U08 ID, U08 Instruction, U08 ParameterLength)
{
     U08 Count,CheckSum,PacketLength;



     TxBuffer[0] = 0xff;
     TxBuffer[1] = 0xff;
     TxBuffer[2] = ID;
     TxBuffer[3] = ParameterLength+2;
     TxBuffer[4] = Instruction;
     for( Count = 0; Count < ParameterLength ; Count++ )
     {
          TxBuffer[Count+5] = Parameter[Count];
     }
     CheckSum = 0;

     PacketLength = ParameterLength +4+2;



     for( Count = 2; Count < PacketLength-1; Count++ ) //except 0xff,checksum
     {
         CheckSum += TxBuffer[Count];
     }
     TxBuffer[Count] = ~CheckSum;

     RS485_TXD;


     for(Count = 0; Count < PacketLength; Count++)
     {
         sbi(UCSR0A,6);       //SET_TXD0_FINISH;
         Uart_Putch(UART0,(TxBuffer[Count]));
     }
     while(!CHECK_TXD0_FINISH); //Wait until TXD Shift register empty


     RS485_RXD;
	


     return(PacketLength);
}

//Plays the loser sounds
void play_loser(void)
{
  //printf("You failed!\n", 0);

  sbi(LED_RED_PORT, LED_RED);
  sbi(LED_GREEN_PORT, LED_GREEN);
  toner('0', 255);

  sbi(LED_BLUE_PORT, LED_BLUE);
  sbi(LED_YELLOW_PORT, LED_YELLOW);
  toner('0', 255);

  sbi(LED_RED_PORT, LED_RED);
  sbi(LED_GREEN_PORT, LED_GREEN);
  toner('0', 255);

  sbi(LED_BLUE_PORT, LED_BLUE);
  sbi(LED_YELLOW_PORT, LED_YELLOW);
  toner('0', 255);
}

void init(void)
{
	//Make outputs low
	cbi(PWM_PORT, PWM_BIT);
	cbi(SD_PORT, PWM_BIT);
	
	//Make outputs outputs
	sbi(PWM_DDR, PWM_BIT);

	//Pull-up swithces
	sbi(ESTOP_PORT, ESTOP_BIT);
	sbi(BRAKE_PORT, BRAKE_BIT);
	
	//RGB Outputs
	sbi(RGB_DDR, RED);
	sbi(RGB_DDR, GREEN);
	sbi(RGB_DDR, BLUE);
	
	//RGB Off
	sbi(RGB_PORT, RED);
	sbi(RGB_PORT, GREEN);
	sbi(RGB_PORT, BLUE);
	
	//Initialise the ADC 
	a2dInit();
	
	//Initialise timer 1
	timer1Init();
	timer1SetPrescaler(TIMER_CLK_DIV1);
	timer1PWMInitICR(TOP_COUNT); //Enable PWM with top count
	timer1PWMAOn(); //Turn PWM on
	
	rgb(green);

	sei(); //Enable interupts
}

u08 ax88796Read(u08 address)
{
	u08 data;
   
	// assert the address
    AX88796_ADDRESS_PORT = address | (AX88796_ADDRESS_PORT&~AX88796_ADDRESS_MASK);

	// assert read
	cbi(AX88796_CONTROL_PORT, AX88796_CONTROL_READPIN);
	nop();
	nop();
	// read in the data
	data = AX88796_DATA_PIN;

	// negate read
	sbi(AX88796_CONTROL_PORT, AX88796_CONTROL_READPIN);

	return data;
}