#include <SoftwareSerial.h>  //Software Serial Port
#include <avr/io.h>
#include <util/delay.h>


#define RxD 2
#define TxD 1

#define led_onboard 0

#define LED_DATA_PORT PA3

/**/ // I/O 3 cycles //Works
//Send 0
#define send0() PORTA |= (1 << LED_DATA_PORT); PORTA &= ~(1 << LED_DATA_PORT); asm volatile("nop \n nop \n nop \n nop");
//Send 1
#define send1() PORTA |= (1 << LED_DATA_PORT); asm volatile("nop \n nop \n nop"); PORTA &= ~(1 << LED_DATA_PORT); asm volatile("nop");
/**/
 
SoftwareSerial blueToothSerial(RxD,TxD);
unsigned long int data = 0;



 
void setup() 
{ 
  // Set the LED port number as output.
  DDRA |= (1 << LED_DATA_PORT);
  //Init LED data pin with 0
  PORTA &= ~(1 << LED_DATA_PORT);
  
  pinMode(RxD, INPUT);
  pinMode(TxD, OUTPUT);
  setupBlueToothConnection();
  
  pinMode(led_onboard,OUTPUT);
  digitalWrite(led_onboard,HIGH);
} 
 
void loop() 
{ 
  char recvChar;
  
  while(1){
    
    //blueToothSerial.println("Bluetooth Test"); //good for debugging your connection
    //delay(1000); 
    
    while(blueToothSerial.available()){
        recvChar = blueToothSerial.read();

        if ( recvChar == '1' ) {
            for (char i = 0; i<25; i++) {
              //LED1
              // GREEN
              send0(); send0(); send0(); send1(); send0(); send0(); send0(); send0(); 
              // RED
              send0(); send0(); send0(); send1(); send0(); send0(); send0(); send0(); 
              // BLUE
              send0(); send0(); send0(); send1(); send0(); send0(); send0(); send0(); 
            }
        }
        if ( recvChar == '0' ) {
            for (char i = 0; i<25; i++) {
              //LED1
              // GREEN
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
              // RED
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
              // BLUE
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
            }
        }
        if ( recvChar == 'r' ) {
            for (char i = 0; i<25; i++) {
              //LED1
              // GREEN
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
              // RED
              send0(); send0(); send0(); send1(); send0(); send0(); send0(); send0(); 
              // BLUE
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
            }
        }
        if ( recvChar == 'g' ) {
            for (char i = 0; i<25; i++) {
              //LED1
              // GREEN
              send0(); send0(); send0(); send1(); send0(); send0(); send0(); send0(); 
              // RED
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
              // BLUE
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
            }
        }
        if ( recvChar == 'b' ) {
            for (char i = 0; i<25; i++) {
              //LED1
              // GREEN
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
              // RED
              send0(); send0(); send0(); send0(); send0(); send0(); send0(); send0(); 
              // BLUE
              send0(); send0(); send0(); send1(); send0(); send0(); send0(); send0(); 
            }
        }
        // RESET to light up the LED
        PORTA &= ~(1 << LED_DATA_PORT); _delay_us(50);
        
        
        
    }
    /*while(blueToothSerial.available()){
        recvChar = blueToothSerial.read();

        if ( recvChar == ' ' ) {
            // assume correct data was received and can be taken to drive the led, everything else would need too much memory
            unsigned char r = 0;
            unsigned char g = 0;
            unsigned char b = 0;

            b = data & 0b11111111;
            data = data >> 8;
            g = data & 0b11111111;
            data = data >> 8;
            r = data & 0b11111111;
            data = data >> 8;

            data = 0;
            
            //led(255,0,0);
            digitalWrite(led_onboard,HIGH);
            delay(1000);
        }
        else {
            data *= 10;
            data += recvChar - '0';
            
            //led(0,0,0);
            digitalWrite(led_onboard,LOW);
            delay(1000);
        }
        
        
        
    }*/
  }
} 
 
void setupBlueToothConnection()
{
  blueToothSerial.begin(9600); //Set BluetoothBee BaudRate to default baud rate 9600 38400
  blueToothSerial.print("\r\n+STWMOD=0\r\n"); //set the bluetooth work in slave mode
  blueToothSerial.print("\r\n+STNA=HC-05\r\n"); //set the bluetooth name as "HC-05"
  blueToothSerial.print("\r\n+STOAUT=1\r\n"); // Permit Paired device to connect me
  blueToothSerial.print("\r\n+STAUTO=0\r\n"); // Auto-connection should be forbidden here
  
  delay(2000); // This delay is required.
  //blueToothSerial.print("\r\n+INQ=1\r\n"); //make the slave bluetooth inquirable 
  blueToothSerial.print("bluetooth connected!\n");
  
  delay(2000); // This delay is required.
  blueToothSerial.flush();
}