// input device assignment
//
//    115200 baud / FTDI interface
//
// Based Neil Gershenfeld' hellolight.45.c
// 10/27/10
// Yuji OKI
// 06/19/2016
//
// (c) Kyushu Univerisy I&E Visionaries
// This work may be reproduced, modified, distributed,
// performed, and displayed for any purpose. Copyright is
// retained and must be preserved. The work is provided
// as is; no warranty is provided, and users accept all 
// liability.
//

//#define cbi(addr,bit)     addr &= ~(1<<bit)
//#define sbi(addr,bit)     addr |=  (1<<bit)


#include <avr/io.h>
#include <util/delay.h>
#include <avr/pgmspace.h>

#define output(directions,pin) (directions |= pin) // set port direction for output
#define input(direction,pin) (direction &= (~pin)) 
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set
#define bit_delay_time 8.5 // bit delay for 115200 with overhead
//#define bit_delay_time 102 // bit delay for 9600 with overhead
#define bit_delay() _delay_us(bit_delay_time) // RS232 bit delay
#define half_bit_delay() _delay_us(bit_delay_time/2) // RS232 half bit delay
#define char_delay() _delay_ms(10) // char delay

#define serial_port PORTA
#define serial_direction DDRA
#define serial_pins PINA
#define serial_pin_in (1 << PA0)
#define serial_pin_out (1 << PA1)
//#define serial_pin_out (1 << PB2)

#define ledR_port PORTA
#define ledG_port PORTB
#define ledB_port PORTA
#define ledR_direction DDRA
#define ledG_direction DDRB
#define ledB_direction DDRA
#define ledR_pin PA6
#define ledG_pin PB2
#define ledB_pin PA5

#define offBlue set(ledB_port, (1<<ledB_pin));
#define onBlue clear(ledB_port,(1<<ledB_pin));
#define offGreen set(ledG_port,(1<<ledG_pin));
#define onGreen clear(ledG_port,(1<<ledG_pin));
#define offRed set(ledR_port,(1<<ledR_pin));
#define onRed clear(ledR_port,(1<<ledR_pin));

#define pdR_port PORTA
#define pdG_port PORTA
#define pdB_port PORTA
#define pdR_pin PA7
#define pdG_pin PA3
#define pdB_pin PA2
#define pdR_direction DDRA
#define pdG_direction DDRA
#define pdB_direction DDRA

#define max_buffer 25


static int blueflag=0;

void get_char(volatile unsigned char *pins, unsigned char pin, char *rxbyte) {
   //
   // read character into rxbyte on pins pin
   //    assumes line driver (inverts bits)
   //
   *rxbyte = 0;
   while(pin_test(*pins,pin)) ;
      //
      // wait for start bit
      //
   //
   // delay to middle of first data bit
   //
   half_bit_delay();
   bit_delay();
   //
   // unrolled loop to read data bits
   //
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 0);
   else
      *rxbyte |= (0 << 0);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 1);
   else
      *rxbyte |= (0 << 1);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 2);
   else
      *rxbyte |= (0 << 2);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 3);
   else
      *rxbyte |= (0 << 3);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 4);
   else
      *rxbyte |= (0 << 4);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 5);
   else
      *rxbyte |= (0 << 5);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 6);
   else
      *rxbyte |= (0 << 6);
   bit_delay();
   if pin_test(*pins,pin)
      *rxbyte |= (1 << 7);
   else
      *rxbyte |= (0 << 7);
   //
   // wait for stop bit
   //
   bit_delay();
   half_bit_delay();
}
   

void put_char(volatile unsigned char *port, unsigned char pin, char txchar) {
   //
   // send character in txchar on port pin
   //    assumes line driver (inverts bits)
   //
   // start bit
   //
   clear(*port,pin);
   bit_delay();
   //
   // unrolled loop to write data bits
   //
   if bit_test(txchar,0)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,1)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,2)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,3)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,4)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,5)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,6)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,7)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   //
   // stop bit
   //
   set(*port,pin);
   bit_delay();
   //
   // char delay
   //
   bit_delay();
}

void put_string(volatile unsigned char *port, unsigned char pin, char *str) {
   //
   // print a null-terminated string
   //
   static int index;
   index = 0;
   do {
      put_char(port, pin, str[index]);
      ++index;
      } while (str[index] != 0);
   }


// AD conversion function
// Arg:adch is ADC number
// This is not PORT number, but ADC number such as [ADC 0-8]

uint16_t singleADC(uint8_t adch, uint16_t nos, uint16_t div)
{
	uint16_t ret;
	uint16_t count;
	uint32_t sum;
	ADMUX = 0b10100000;
	for(count=0;count<1000;count++){
    set(ADCSRA,(1 << ADIF));//reset flag
	set(ADCSRA, (1 << ADSC));//start ADC. ADSC(A/D Conv start reg.)
	while (ADCSRA & (1 << ADSC));//Wait for flag=1
	}
//	clear(ADCSRA,(1<<ADEN));
    ADMUX = 0b10000000;
//    Following process DOES NOT WORK
//    clear (ADMUX,(1<<MUX5)|(1<<MUX4)|(1<<MUX3)| (1<<MUX2)|(1<<MUX1)|(1<<MUX0));
//    set(ADMUX, (adch & (1<<MUX2)|(1<<MUX1)|(1<<MUX0))); //  LSB0,1,2 ->adch
//    if(adch&0b10000000) ADMUX = adch;
	ADMUX = ((1 << REFS1) | (0 << REFS0)  | adch );// 1.1V -> Vref.
    set(MCUCR, (1<<PUD));
//    set(ADCSRA, (1<< ADEN));
	sum=0;
	for(count=0;count<nos;count++){
 	   set(ADCSRA,(1 << ADIF));
 	   		set(ADCSRA, (1 << ADSC));
		while (ADCSRA & (1 << ADSC));
		sum+=(uint32_t)ADC;
	}
	while (ADCSRA & (1 << ADIF)==0);// wait for end of conversion
    ret=(uint16_t)(sum/div);

    return ret;
}

void put_number(volatile unsigned char *port, unsigned char pin, uint16_t number)
{
	  put_char(port, pin, ((uint32_t)number /10000)+48);
//      char_delay();
      put_char(port, pin, ((number%10000) /1000)+48);
//      char_delay();
      put_char(port, pin,((number%1000) /100)+48);
//      char_delay();
      put_char(port, pin, ((number%100) /10)+48);
//      char_delay();
      put_char(port, pin, (number%10)+48);
}
int get_string(volatile unsigned char *port, unsigned char pin, unsigned char *string, unsigned int length)
{
  int count=0;
  unsigned char chr;
  do{
    get_char(&serial_pins, serial_pin_in, string);
  }while(*string++!=13 && count++<length);
  *string=0;
  return count;
}

int main(void) {
   //
   // main
   //
   static char chr;
   static unsigned int value; 
   static char buffer[max_buffer] = {0};
   static int index;
   static int nsamples=1;
   static uint32_t accum;
   static uint16_t level_r;
   static uint16_t level_g;
   static uint16_t level_b;
   static int count;
   int loopcount=0;
   uint8_t bflag=0;
   uint8_t gflag=0;
   uint8_t rflag=0;
   uint8_t btconnect=0;

   //
   // set clock divider to /1
   //
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   // initialize output pins
   //
   set(serial_port, serial_pin_out);
   output(serial_direction, serial_pin_out);

   //
   // initialize LED pins
   //
   set(ledR_port, (1<<ledR_pin));
   output(ledR_direction, (1<<ledR_pin));
   set(ledG_port, (1<<ledG_pin));
   output(ledG_direction, (1<<ledG_pin));
   set(ledB_port, (1<<ledB_pin));
   output(ledB_direction, (1<<ledB_pin));

   clear(PORTA, (1<< pdR_pin));
   input(pdR_direction,(1<<pdR_pin));
   clear (PORTA, (1<< pdG_pin));
   input(pdG_direction, (1<<pdG_pin));
   clear (PORTA, (1<< pdB_pin));
   input(pdB_direction, (1<<pdB_pin));
   //set(MCUCR, (1<<PUD));

   offGreen;
   offRed;
   offBlue;

   //
   // init A/D
   //
   ADMUX = (1 << REFS1) | (0 << REFS0) // Vcc is as Vref
      | (0 << MUX5) | (0 << MUX4) | (0 << MUX3) | (1 << MUX2) | (1 << MUX1) | (1 << MUX0);
  //   MUX5=1, MUX4=0 -> 0V input;
  //   ADMUX = 0b10000011;
   ADCSRA = (1 << ADEN)
       | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); //  CK/128‚É
   set(DIDR0, (1<<ADC7D)|(1<<ADC3D)|(1<<ADC2D));
   do{
     put_string(&serial_port, serial_pin_out,"$$$\r");
     char_delay();
     get_string(&serial_port, serial_pin_out,buffer, max_buffer);
     if (strcmp("CMD\r",buffer)==0){
       do{
	 do{
	   put_string(&serial_port, serial_pin_out,"C\r");
	   char_delay();
	   get_string(&serial_port, serial_pin_out,buffer, max_buffer);
	   char_delay();
	 }while(strcmp("TRYING\r",buffer));
	 get_string(&serial_port, serial_pin_out,buffer, max_buffer);
	 char_delay();
	 btconnect=!strcmp("CONNECTED\r",buffer);
       }while(btconnect==0);
     }
     char_delay();
   }while (btconnect==0);
   //
   // main loop
   //
   while (1) {
     get_string(&serial_port, serial_pin_out,buffer, max_buffer);
      //
      // ADC stack data
      //
 	  level_g = singleADC(3,10,3);
	  level_b = singleADC(2,10,5);
	  level_r = singleADC(7,16,2);

	  put_char(&serial_port, serial_pin_out,'R');
	  put_char(&serial_port, serial_pin_out,':');
	  put_number(&serial_port, serial_pin_out,level_r);
	  	  char_delay();
	  put_char(&serial_port, serial_pin_out,',');
	  put_char(&serial_port, serial_pin_out,'G');
	  	  put_char(&serial_port, serial_pin_out,':');
	 	  put_number(&serial_port, serial_pin_out,level_g);
	  	  char_delay();
	  put_char(&serial_port, serial_pin_out,',');
	  put_char(&serial_port, serial_pin_out,'B');
	  	  put_char(&serial_port, serial_pin_out,':');
	  put_number(&serial_port, serial_pin_out,level_b);

	  char_delay();
      put_char(&serial_port, serial_pin_out,13);
      char_delay();

       }
   }
   
   //
   // main loop

// G : r=70 g=1420 b=202
// R:  r=1360, g=20, b=10
// B:  r=40  g=197 B=1360
// BG:  r=94 g=1650  B=1563
// BR : r=1370 213 1353
// GR : 1420 1489 206
//RGB : 1429 1699 1564