void init_NRF24L01(void)
{
    //init SPI
    SPCR=0x51; //set this to 0x50 for 1 mbits 
    SPSR=0x00; 
    
    //inerDelay_us(100);
    _delay_us(100);
 	cbi(PORTC,CE);//CE=0;    // chip enable
 	sbi(PORTC,CSN);//CSN=1;   // Spi disable
 	//SCK=0;   // Spi clock line init high
	SPI_Write_Buf(WRITE_REG + TX_ADDR, TX_ADDRESS, TX_ADR_WIDTH);    //
	SPI_Write_Buf(WRITE_REG + RX_ADDR_P0, RX_ADDRESS, RX_ADR_WIDTH); //
	SPI_RW_Reg(WRITE_REG + EN_AA, 0x00);      // EN P0, 2-->P1
	SPI_RW_Reg(WRITE_REG + EN_RXADDR, 0x01);  //Enable data P0
	SPI_RW_Reg(WRITE_REG + RF_CH, 2);        // Chanel 0 RF = 2400 + RF_CH* (1or 2 M)
	SPI_RW_Reg(WRITE_REG + RX_PW_P0, RX_PLOAD_WIDTH); // Do rong data truyen 32 byte
	SPI_RW_Reg(WRITE_REG + RF_SETUP, 0x07);   		// 1M, 0dbm
	SPI_RW_Reg(WRITE_REG + CONFIG, 0x0e);   		 // Enable CRC, 2 byte CRC, Send

}

void setup()
{
	cbi(TIMSK0, TOIE0);

	////////////////////////////
	pinMode(A0, OUTPUT);
	pinMode(A1, OUTPUT);

	pinMode(A2, INPUT);
	pinMode(A3, INPUT);
	pinMode(A4, INPUT);
	pinMode(A5, INPUT);

	digitalWrite(A0, LOW);
	digitalWrite(A1, HIGH);

	Serial.begin(9600);

	memset(color, 0x00, LENGTH);
	tm.sendData(color, LENGTH);
}

//! Writes a byte of data to the LCD.
static void writeLcd(u08 data)
{
	//Reverse the bit order of the data, due to LCD connections to the data bus being backwards.
	//This line doesn't affect the bus, so this can be done before the interrupts are disabled.
	REVERSE(data);
	//Disable interrupts in this block to prevent the servo ISR (which shares the same data bus)
	//from interrupting in the middle of the sequence and messing with the bus.
	//ATOMIC_RESTORESTATE is used so that this function can be called from
	//user code/ISRs without unexpected side effects.
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		//set the LCD's E (Enable) line high, so it can fall later
		sbi(PORTD, PD6);
		//write the data to the bus
		PORTC = data;
		//brief delay to allow the data to fully propagate to the LCD
		delayUs(1);
		//set the LCD's E (Enable) line low to latch in the data
		cbi(PORTD, PD6);
	}
}

int main(void)
{    
	DDRB=0XC7;						//SET DATA DIRECTION REGISTER
									//SET 1 for OUTPUT PORT
									//SET 0 FOR INPUT PORT
									//PB.2 IS  INPUT
									//ALL OTHERS ARE OUTPUT
	DDRD=0;
	//DDRD=0XF1;						//SET DATA DIRECTION REGISTER
									//SET 1 for OUTPUT PORT
									//SET 0 FOR INPUT PORT
									//PD.1, PD.2 AND PD.3 ARE INPUT
									//ALL OTHERS ARE OUTPUT

	sbi(PORTD,2);					//ENABLE PULL UP FOR SWITCH INT0
	sbi(PORTD,3);					//ENABLE PULL UP FOR SWITCH INT1
    GICR = _BV(INT0);           	// enable external int0
    MCUCR = _BV(ISC01);          	// falling egde: int0*/
    sei();                       	// enable interrupts 
    for (;;) 					 	//BLINK LED2
	{	
	
	
    _delay_ms(500);
	cbi(PORTB,2);			//LED OFF
	
		setstate('1','F');
		_delay_ms(1000);
		setstate('2','B');
		_delay_ms(1000);

	
	
//    _delay_ms(2000);
//	motormove('2','F');
//    _delay_ms(2000);
//	motormove('1','F');
//    _delay_ms(2000);
    }
}

//brake selected motor
//full brake assumed
void brake(uint8_t motorSelect)
{
   switch(motorSelect)
   {
      case LEFT_MOTOR:
         cbi(PORTD, REVERSE_LEFT);
         cbi(PORTD, FORWARD_LEFT);
         break;
      case RIGHT_MOTOR:
         cbi(PORTD, REVERSE_RIGHT);
         cbi(PORTD, FORWARD_RIGHT);
         break;
      case BOTH:
         // both motors reverse
         cbi(PORTD, REVERSE_LEFT);
         cbi(PORTD, REVERSE_RIGHT);
         cbi(PORTD, FORWARD_LEFT);
         cbi(PORTD, FORWARD_RIGHT);
         break;
   }
   PWM_RIGHT(MAX_BRAKE);
   PWM_LEFT(MAX_BRAKE);
}

//Plays the winner sounds
void play_winner(void)
{
  uint8_t x, y, z;

  //printf("You win!\n", 0);

  sbi(LED_RED_PORT, LED_RED);
  sbi(LED_YELLOW_PORT, LED_YELLOW);
  sbi(LED_BLUE_PORT, LED_BLUE);
  sbi(LED_GREEN_PORT, LED_GREEN);

  for(z = 0 ; z < 4 ; z++)
    {
      if(z == 0 || z == 2)
        {
          cbi(LED_RED_PORT, LED_RED);
          cbi(LED_YELLOW_PORT, LED_YELLOW);
          sbi(LED_BLUE_PORT, LED_BLUE);
          sbi(LED_GREEN_PORT, LED_GREEN);
        }
      else
        {
          sbi(LED_RED_PORT, LED_RED);
          sbi(LED_YELLOW_PORT, LED_YELLOW);
          cbi(LED_BLUE_PORT, LED_BLUE);
          cbi(LED_GREEN_PORT, LED_GREEN);
        }

      for(x = 250 ; x > 70 ; x--)
        {
          for(y = 0 ; y < 3 ; y++)
            {
              //Toggle the buzzer at various speeds
              sbi(BUZZER2_PORT, BUZZER2);
              cbi(BUZZER1_PORT, BUZZER1);

              delay_us(x);

              cbi(BUZZER2_PORT, BUZZER2);
              sbi(BUZZER1_PORT, BUZZER1);

              delay_us(x);
            }
        }
    }

}

void path_follower(void)
{
	
	if((bit_is_clear(PINC,2)) && (bit_is_clear(PINC,3)))
	{
		sbi(PORTD,0);
		cbi(PORTD,1);	//move left
		//robotmove('L');
		sbi(PORTD,7);
		cbi(PORTB,0);
	}
	if((bit_is_clear(PINB,1)) && (bit_is_clear(PINC,5)))
	{
		sbi(PORTD,1);
		cbi(PORTD,0);	//move right
		//robotmove('R');
		sbi(PORTB,0);
		cbi(PORTD,7);
	}
	if((bit_is_clear(PINC,3)) && (bit_is_clear(PINC,4)) && (bit_is_clear(PINC,5)))
	{
		sbi(PORTD,0);
		sbi(PORTD,1);	//move forward
		//robotmove('F');
		sbi(PORTD,7);
		sbi(PORTB,0);
	}
	if((!bit_is_clear(PINC,2)) && (!bit_is_clear(PINC,3)) && (!bit_is_clear(PINC,4)) && (!bit_is_clear(PINC,5)) && (!bit_is_clear(PINB,1)))
	{
		cbi(PORTD,0);
		cbi(PORTD,1);	//stop
		//robotmove('S');
		cbi(PORTD,7);
		cbi(PORTB,0);
	}
	
}

// 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();
}

void HardwareSerial::begin(unsigned long baud, byte config)
{
    // Try u2x mode first
    uint16_t baud_setting = (F_CPU / 4 / baud - 1) / 2;
    *_ucsra = 1 << U2X0;

    // hardcoded exception for 57600 for compatibility with the bootloader
    // shipped with the Duemilanove and previous boards and the firmware
    // on the 8U2 on the Uno and Mega 2560. Also, The baud_setting cannot
    // be > 4095, so switch back to non-u2x mode if the baud rate is too
    // low. Disable to get a more accurate baud rate at 57600
//#define EXCEPTION_FOR_57600
#if defined (EXCEPTION_FOR_57600)
    if (((F_CPU == 16000000UL) && (baud == 57600)) || (baud_setting >4095))
#else
    if (baud_setting >4095)
    {
        *_ucsra = 0;
        baud_setting = (F_CPU / 8 / baud - 1) / 2;
    }
#endif

        // assign the baud_setting, a.k.a. ubrr (USART Baud Rate Register)
        *_ubrrh = baud_setting >> 8;
    *_ubrrl = baud_setting;

    _written = false;

    //set the data bits, parity, and stop bits
#if defined(__AVR_ATmega8__)
    config |= 0x80; // select UCSRC register (shared with UBRRH)
#endif
    *_ucsrc = config;

    sbi(*_ucsrb, RXEN0);
    sbi(*_ucsrb, TXEN0);
    sbi(*_ucsrb, RXCIE0);
    cbi(*_ucsrb, UDRIE0);
}

bool resetXBeeSoft(void)
{
    uint8_t messageState = 0;   /* Progress in message reception */
    rxFrameType rxMessage;
    uint8_t count = 0;
    bool reset = false;
    while (! reset)
    {
        sendATFrame(2,"FR");

/* The frame type we are handling is 0x88 AT Command Response */
        uint16_t timeout = 0;
        messageState = 0;
        uint8_t messageError = XBEE_INCOMPLETE;
/* Wait for response. If it doesn't come, try sending again. */
        while (messageError == XBEE_INCOMPLETE)
        {
            messageError = receiveMessage(&rxMessage, &messageState);
            if (timeout++ > 30000) break;
        }
/* If errors occur, or frame is the wrong type, just try again */
        reset = ((messageError == XBEE_COMPLETE) && \
                 (rxMessage.message.atResponse.status == 0) && \
                 (rxMessage.frameType == AT_COMMAND_RESPONSE) && \
                 (rxMessage.message.atResponse.atCommand1 == 'F') && \
                 (rxMessage.message.atResponse.atCommand2 == 'R'));
#ifdef TEST_PORT
        if (! reset)
        {
            _delay_ms(200);
            sbi(TEST_PORT,TEST_PIN);    /* Set pin on */
            _delay_ms(200);
            cbi(TEST_PORT,TEST_PIN);    /* Set pin off */
        }
#endif
        if (count++ > 10) break;
    }
    return reset;
}

void HardwareSerial::begin(unsigned long baud)
{
    uint16_t baud_setting;
    bool use_u2x = true;

#if F_CPU == 16000000UL
    // hardcoded exception for compatibility with the bootloader shipped
    // with the Duemilanove and previous boards and the firmware on the 8U2
    // on the Uno and Mega 2560.
    if (baud == 57600) {
        use_u2x = false;
    }
#endif

try_again:

    if (use_u2x) {
        *_ucsra = 1 << _u2x;
        baud_setting = (F_CPU / 4 / baud - 1) / 2;
    } else {
        *_ucsra = 0;
        baud_setting = (F_CPU / 8 / baud - 1) / 2;
    }

    if ((baud_setting > 4095) && use_u2x)
    {
        use_u2x = false;
        goto try_again;
    }

    // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register)
    *_ubrrh = baud_setting >> 8;
    *_ubrrl = baud_setting;

    sbi(*_ucsrb, _rxen);
    sbi(*_ucsrb, _txen);
    sbi(*_ucsrb, _rxcie);
    cbi(*_ucsrb, _udrie);
}

void BiscuitSerial::begin(unsigned long baud)
{
  uint16_t baud_setting;
  bool use_u2x = true;

try_again:
  
  if (use_u2x) {
    UCSR0A = 1 << U2X0;
    baud_setting = (F_CPU / 4 / baud - 1) / 2;
  } else {
    UCSR0A = 0;
    baud_setting = (F_CPU / 8 / baud - 1) / 2;
  }
  
  if ((baud_setting > 4095) && use_u2x)
  {
    use_u2x = false;
    goto try_again;
  }

  // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register)
  UBRR0H = baud_setting >> 8;
  UBRR0L = baud_setting;

  sbi(UCSR0B, RXEN0);
  sbi(UCSR0B, TXEN0);
  sbi(UCSR0B, RXCIE0);  
  cbi(UCSR0B, UDRIE0);

  // setup 9bit
  /*
  sbi(UCSR0C, UCSZ00);
  sbi(UCSR0C, UCSZ01);
  sbi(UCSR0B, UCSZ02);
  */
  // setup even parity
  //sbi(UCSR0C, UPM01);
}

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
	#ifdef UARTSW_INVERT
	sbi(UARTSW_TX_PORT, UARTSW_TX_PIN);
	#else
	cbi(UARTSW_TX_PORT, UARTSW_TX_PIN);
	#endif
	// schedule the next bit
	outb(OCR2, inb(TCNT2) + UartswBaudRateDiv);
	// enable OC2 interrupt
	sbi(TIMSK, OCIE2);
}

//------------------------------------------------------------------------------
//     				===== Uart_Init =====
//             		: 희망하는 속도로 시리얼 통신을 초기화 한다.
//------------------------------------------------------------------------------
void Init_Uart(U08 Com, U32 Uart_Baud)
{
	U16 Temp_UBRR;

	Temp_UBRR = AVR_CLK/(16L * Uart_Baud) - 1;   	// 통신 보레이트 계산식
													// U2X = 0 일때 (일반모드)
	
    //---------------------------- UART0 초기화 --------------------------------
	if( Com == UART0 )                           	
	{
		UBRR0H = (Temp_UBRR >> 8);              // 통신속도 설정
		UBRR0L = (Temp_UBRR & 0x00FF);
		
		UCSR0A = (0<<RXC0)  | (1<<UDRE0);		// 수신,송신 상태비트 초기화
        UCSR0B = (1<<RXEN0) | (1<<TXEN0);  		// 수신,송신 기능 활성화
		UCSR0C = (3<<UCSZ00);				// START 1비트/DATA 8비트/STOP 1비트
		
		cbi( DDRE, 0 );                         // RXD0 핀 입력으로 설정
		sbi( DDRE, 1 );                         // TXD0 핀 출력으로 설정
		
		UCSR0B |=  (1<<RXCIE0);	             	// 수신인터럽트0 활성화
	}

void DoAConversion(void)
{
   if (ShwattDaqState & WaitingForXaxis)
   {
      lastDaqTime_ = currentDaqTime_;
      ADMUX = XaxisPinMuxValue; //set pin to read
      sbi(ADCSRA, ADSC);        //start ADC
   } else if (ShwattDaqState & WaitingForZaxis)
   {
      ADMUX = ZaxisPinMuxValue;
      sbi(ADCSRA, ADSC);
   } else if (ShwattDaqState & WaitingForYrate)
   {
      ADMUX = YratePinMuxValue;
      sbi(ADCSRA, ADSC);
   } else
   {
      cbi(ADCSRA, ADEN);   //disable ADC
      ShwattDaqComplete(); //handler function for the end of the DAQ
   }

}

void twi_close()
{	
	// de-activate internal pull-up resistors
	cbi(PORTD, 0);
	cbi(PORTD, 1);
	
	
	sbi(TWSR, TWPS0);
	sbi(TWSR, TWPS1);
	
  // disable twi module, acks, and twi interrupt
	sbi(TWCR,TWINT);
	sbi(TWCR,TWSTO);
	cbi(TWCR,TWEA);
	cbi(TWCR,TWSTA);
	cbi(TWCR,TWWC);
	sbi(TWCR,TWEN);
	cbi(TWCR,TWIE);
	cbi(PRR0,PRTWI);
}

//! initializes software PWM system
void servoInit(void)
{
	u08 channel;
	// disble the timer1 output compare A interrupt
	cbi(TIMSK, OCIE1A);
	// set the prescaler for timer1
	timer1SetPrescaler(TIMER_CLK_DIV256);
	// attach the software PWM service routine to timer1 output compare A
	timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
	// enable and clear channels
	for(channel=0; channel<SERVO_NUM_CHANNELS; channel++)
	{
		// set minimum position as default
		ServoChannels[channel].duty = SERVO_MIN;
		// set default port and pins assignments
		ServoChannels[channel].port = _SFR_IO_ADDR(SERVO_DEFAULT_PORT);
		//ServoChannels[channel].port = (unsigned char)&SERVO_DEFAULT_PORT;
		ServoChannels[channel].pin = (1<<channel);
		// set channel pin to output
		// THIS IS OBSOLETED BY THE DYNAMIC CHANNEL TO PORT,PIN ASSIGNMENTS
		//outb(SERVODDR, inb(SERVODDR) | (1<<channel));
	}
	// set PosTics
	ServoPosTics = 0;
	// set PeriodTics
	ServoPeriodTics = SERVO_MAX*9;
	// set initial interrupt time
	u16 OCValue;
	// read in current value of output compare register OCR1A
	OCValue =  inb(OCR1AL);		// read low byte of OCR1A
	OCValue += inb(OCR1AH)<<8;	// read high byte of OCR1A
	// increment OCR1A value by nextTics
	OCValue += ServoPeriodTics; 
	// set future output compare time to this new value
	outb(OCR1AH, (OCValue>>8));			// write high byte
	outb(OCR1AL, (OCValue & 0x00FF));	// write low byte
	// enable the timer1 output compare A interrupt
	sbi(TIMSK, OCIE1A);
}

int main(void)
{
	
	DDRA=0xF0;						//SET DATA DIRECTION REGISTER
									//SET 1 for OUTPUT PORT
									//SET 0 FOR INPUT PORT
									//PA.4, PA.5, PA.6 AND PA.7 ARE OUTPUT
									//ALL OTHERS ARE INPUT
									
	DDRB=0XFB;						//SET DATA DIRECTION REGISTER
									//SET 1 for OUTPUT PORT
									//SET 0 FOR INPUT PORT
									//PB.2 IS  INPUT
									//ALL OTHERS ARE OUTPUT
	
	DDRD=0XF1;						//SET DATA DIRECTION REGISTER
									//SET 1 for OUTPUT PORT
									//SET 0 FOR INPUT PORT
									//PD.1, PD.2 AND PD.3 ARE INPUT
									//ALL OTHERS ARE OUTPUT

	sbi(PORTA,4);					//LED1 ON (INDICATION FOR READY TO USE)
	
	
	sbi(PORTB,2);					//ENABLE PULL UP FOR SWITCH INT2
	sbi(PORTD,1);					//ENABLE PULL UP FOR SW1
	sbi(PORTD,2);					//ENABLE PULL UP FOR SWITCH INT0
	sbi(PORTD,3);					//ENABLE PULL UP FOR SWITCH INT1
    

    for (;;)								/* loop forever */ 
	{                           
		sbi(PORTA,5);
        wait_debounce();				/* wait until push button sw1 is pressed */
		cbi(PORTA,5);
		wait_debounce();				/* wait until push button sw1 is pressed */
    }
}

static int SpiMemInit(uint8_t cs, uint16_t *pages, uint16_t *pagesize)
{
    uint8_t fs;
    
    /* Init SPI memory chip select. */
    if (cs == SPIMEM_CS_BIT) {
        cbi(SPIMEM_CS_PORT, cs);
    } else {
        sbi(SPIMEM_CS_PORT, cs);
    }
    sbi(SPIMEM_CS_DDR, cs);
    
    /* Initialize the SPI interface. */
    SpiInit();
    
    /* Read the status register for a rudimentary check. */
    fs = SpiMemStatus(cs);
    if(fs & 0x80) {
        fs = (fs >> 2) & 0x0F;
        *pagesize = 264;
        if(fs == 3) {
            *pages = 512;
            return 0;
        }
        else if(fs == 5) {
            *pages = 1024;
            return 0;
        }
        else if(fs == 7) {
            *pages = 2048;
            return 0;
        }
        else if(fs == 13) {
            *pagesize = 528;
            *pages = 8192;
            return 0;
        }
    }

size_t FTDI_FT245::write(uint8_t c)
{
  // check state of TXE line
  // TXE = L  => Tx fifo is ready to be written
  // TXE = H  => Tx fifo full or not ready
  while(*_cont_pin & (1<<_txe))
    ;	

  sbi(*_cont_port, _wr);
  *_data_ddr = 0xFF;
  *_data_port = c;
  
  asm volatile ("nop");
  asm volatile ("nop");
  asm volatile ("nop");
  asm volatile ("nop");
  
  cbi(*_cont_port, _wr);
  *_data_ddr = 0x00;  // Put Port DDR Back to Input
  *_data_port = 0xFF;  // Enable The Pull-Ups

  return 1;
}