//
// pad_v2.c
//
// Test for important parts of the code
	/*
	// This code only tests RS232, it returns every received character,
	// use with term.py or BlueTerm2
	get_char(&serial_pins, serial_pin_in, &chr);
	_delay_ms(100);
	output(serial_direction, serial_pin_out);//
	put_char(&serial_port, serial_pin_out, chr);
	put_char(&serial_port, serial_pin_out, 10); // new line
	input(serial_direction, serial_pin_out);//
	*/

	/*
	// This code tests ADC and touchpad's reading,
	// use with medir.py only
	turn_on_red_led();
	for (int i = 0; i < 50; i++){
		sense_and_send_to_medir(left_touchpad);
		//_delay_ms(10);
	}
	turn_on_green_led();
	for (int i = 0; i < 50; i++){
		sense_and_send_to_medir(right_touchpad);
		//_delay_ms(10);
	}
	*/
//

#include <avr/io.h>
#include <util/delay.h>
#include <inttypes.h>
#include <avr/interrupt.h>
#include <stdbool.h>
#include <avr/pgmspace.h>
#include <string.h>

// Original definitions
#define output(directions,pin) (directions |= pin) // set port direction for output
#define input(directions,pin) (directions &= (~pin)) // set port direction for input
#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 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 settle_delay() _delay_us(100) // settle delay
#define char_delay() _delay_ms(10) // char delay

// LED RGB pins' definitions
#define red_port PORTB
#define red_direction DDRB
#define red_pin (1 << PB2)
#define green_port PORTA
#define green_direction DDRA
#define green_pin (1 << PA7)
#define blue_port PORTA
#define blue_direction DDRA
#define blue_pin (1 << PA6)

// Touchpad definitions
#define touchpad_port PORTA
#define touchpad_direction DDRA
#define left_touchpad 'l'
#define left_touchpad_transmitter_pin (1 << PA2)
#define left_touchpad_receiver_pin (1 << MUX1) | (1 << MUX0); // PA3 as ADC
#define right_touchpad 'r'
#define right_touchpad_transmitter_pin (1 << PA0)
#define right_touchpad_receiver_pin (0 << MUX1) | (1 << MUX0); // PA1 as ADC
#define nloop 100 // loops to accumulate

// RS232 bridge definitions
#define serial_port PORTB
#define serial_direction DDRB
#define serial_pins PINB
#define serial_pin_in (1 << PB0) // Bluetooth TXD
#define serial_pin_out (1 << PB1) // Bluetooth RXD
#define node_id '0'
#define start_signal 's'
#define dumbbell_signal 'd'
#define message_signal 'm'
#define change_signal 'c'

// Functions' declarations
void turn_on_red_led(void);
void turn_on_green_led(void);
void turn_on_blue_led(void);
void turn_on_yellow_led(void);
void turn_on_cyan_led(void);
void turn_on_magenta_led(void);
void turn_on_white_led(void);
void turn_off_led(void);
void semaforo(void);
void put_char(volatile unsigned char *port, unsigned char pin, char txchar);
void get_char(volatile unsigned char *pins, unsigned char pin, char *rxbyte);
void put_string(volatile unsigned char *port, unsigned char pin, PGM_P str);
void sense_and_send_to_medir(char touchpad);
uint16_t leer_value(char touchpad);
void send_value(int value);

int main(void) {
	// Variables' declarations
	static char chr;
	static int screen = 0;
	static uint64_t start_time;
	static uint16_t quantity, contador, cont_act;

	// Set clock divider to /1
	CLKPR = (1 << CLKPCE);
	CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);

	// Initialize LED pins
	output(red_direction, red_pin);
	output(green_direction, green_pin);
	output(blue_direction, blue_pin);
	turn_off_led();

	// Initialize RS232 pins
	set(serial_port, serial_pin_out);
	input(serial_direction, serial_pin_out);

	// Initialize touchpad
	clear(touchpad_port, left_touchpad_transmitter_pin);
	clear(touchpad_port, right_touchpad_transmitter_pin);
	output(touchpad_direction, left_touchpad_transmitter_pin);
	output(touchpad_direction, right_touchpad_transmitter_pin);
	ADMUX = 0;
	ADCSRA = (1 << ADEN) // enable
		| (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // prescaler /128
	//input(serial_direction, serial_pin_in);

	// main loop
	while (1) {

		/*// This code tests ADC and touchpad's reading,
		// use with medir.py only
		turn_on_red_led();
		for (int i = 0; i < 50; i++){
			sense_and_send_to_medir(left_touchpad);
			//_delay_ms(10);
		}
		turn_on_green_led();
		for (int i = 0; i < 50; i++){
			sense_and_send_to_medir(right_touchpad);
			//_delay_ms(10);
		}*/

		// This is Chamo's code, do not erase
		get_char(&serial_pins, serial_pin_in, &chr);
		_delay_ms(100);

		if(chr == start_signal)
			screen=0;

		if(chr == change_signal)
			screen=screen+1;

		switch(screen){

			case 0:{

				if(chr == start_signal){
					if(leer_value(left_touchpad) > 12000 && leer_value(right_touchpad)>12000){
						//semaforo();
						output(serial_direction, serial_pin_out);//
						static const char message_1[] PROGMEM = "Start";
			 			put_string(&serial_port, serial_pin_out, (PGM_P) message_1);
						put_char(&serial_port, serial_pin_out, 10); // new line
						input(serial_direction, serial_pin_out);//
					}else{
						output(serial_direction, serial_pin_out);//
						put_char(&serial_port, serial_pin_out, chr);
						put_char(&serial_port, serial_pin_out, 10); // new line
						input(serial_direction, serial_pin_out);//

					}

				}

			}break;

			case 1:{
				int step = 0;
				bool flag = false;

				bool nextR = true;
				bool nextL = false;

				while(step<10){

					bool stepR = (leer_value(right_touchpad)>12000);
					bool stepL = (leer_value(left_touchpad)>12000);

					if((stepR == nextR)&&(stepL == nextL)){
						step = step+1;

						nextR = !stepR;
						nextL = !stepL;
					}

					_delay_ms(100);
				}
				output(serial_direction, serial_pin_out);//
				static const char message_1[] PROGMEM = "Next";
	 			put_string(&serial_port, serial_pin_out, (PGM_P) message_1);
				put_char(&serial_port, serial_pin_out, 10); // new line
				input(serial_direction, serial_pin_out);

			}break;

			case 2:{

			}break;

			case 3:{

			}break;

		}

		if(chr == message_signal){
			// This is part of the group assignment, another project has sent a
			// message and we should answer
			output(serial_direction, serial_pin_out);//
			static const char message_GA[] PROGMEM = "This is Andrea's project";
 			put_string(&serial_port, serial_pin_out, (PGM_P) message_GA);
			put_char(&serial_port, serial_pin_out, 10); // new line
			input(serial_direction, serial_pin_out);//
		}
	}
}

// My Functions' Definitions!
void turn_on_red_led(){
	clear(red_port, red_pin);
	set(green_port, green_pin);
	set(blue_port, blue_pin);
}

void turn_on_green_led(){
	set(red_port, red_pin);
	clear(green_port, green_pin);
	set(blue_port, blue_pin);
}

void turn_on_blue_led(){
	set(red_port, red_pin);
	set(green_port, green_pin);
	clear(blue_port, blue_pin);
}

void turn_on_yellow_led(){
	clear(red_port, red_pin);
	clear(green_port, green_pin);
	set(blue_port, blue_pin);
}

void turn_on_cyan_led(){
	set(red_port, red_pin);
	clear(green_port, green_pin);
	clear(blue_port, blue_pin);
}

void turn_on_magenta_led(){
	clear(red_port, red_pin);
	set(green_port, green_pin);
	clear(blue_port, blue_pin);
}

void turn_on_white_led(){
	clear(red_port, red_pin);
	clear(green_port, green_pin);
	clear(blue_port, blue_pin);
}

void turn_off_led(){
	set(red_port, red_pin);
	set(green_port, green_pin);
	set(blue_port, blue_pin);
}

void semaforo(){
	turn_on_red_led();
	_delay_ms(1000);
	turn_on_yellow_led();
	_delay_ms(1000);
	turn_on_green_led();
	_delay_ms(1000);
	turn_off_led();
}

uint16_t leer_value(char touchpad){
	unsigned char count;
	uint16_t up,down,value;
	uint8_t touchpad_transmitter_pin;
	up = 0;
	down = 0;
	// Specify touchpad pin
	if(touchpad == 'l'){
		// Use left touchpad
		touchpad_transmitter_pin = left_touchpad_transmitter_pin;
		ADMUX = left_touchpad_receiver_pin;
	}else if(touchpad == 'r'){
		// Use right touchpad
		touchpad_transmitter_pin = right_touchpad_transmitter_pin;
		ADMUX = right_touchpad_receiver_pin;
	}else{
		// Bad function call, exit
		return 0;
	}
	// Sense touchpad
	for (count = 0; count < nloop; ++count) {
		// settle, charge
		settle_delay();
		set(touchpad_port, touchpad_transmitter_pin);
		// initiate conversion
		ADCSRA |= (1 << ADSC);
		// wait for completion
		while (ADCSRA & (1 << ADSC))
		;
		// save result
		up += ADC;
		// settle, discharge
		settle_delay();
		clear(touchpad_port, touchpad_transmitter_pin);
		// initiate conversion
		ADCSRA |= (1 << ADSC);
		// wait for completion
		while (ADCSRA & (1 << ADSC))
		;
		// save result
		down += ADC;
	}
	value = up-down;
	return value;
}

void send_value(int value){
	// Enable serial_pin_out
	output(serial_direction, serial_pin_out);
	// Send framing
	put_char(&serial_port, serial_pin_out, 1);
	char_delay();
	put_char(&serial_port, serial_pin_out, 2);
	char_delay();
	put_char(&serial_port, serial_pin_out, 3);
	char_delay();
	put_char(&serial_port, serial_pin_out, 4);
	// Send result
	put_char(&serial_port, serial_pin_out, value);
	char_delay();
	// Disable serial_pin_out
	input(serial_direction, serial_pin_out);
}

void sense_and_send_to_medir(char touchpad){
	//
	// accumulate touchpad magnetic value
	//
	unsigned char count;
	uint16_t up,down;
	uint8_t touchpad_transmitter_pin;
	up = 0;
	down = 0;

	// Specify touchpad pin
	if(touchpad == 'l'){
		// Use left touchpad
		touchpad_transmitter_pin = left_touchpad_transmitter_pin;
		ADMUX = left_touchpad_receiver_pin;
	}else if(touchpad == 'r'){
		// Use right touchpad
		touchpad_transmitter_pin = right_touchpad_transmitter_pin;
		ADMUX = right_touchpad_receiver_pin;
	}else{
		// Bad function call, exit
		return;
	}

	for (count = 0; count < nloop; ++count) {
		// settle, charge
		settle_delay();
		set(touchpad_port, touchpad_transmitter_pin);
		// initiate conversion
		ADCSRA |= (1 << ADSC);
		// wait for completion
		while (ADCSRA & (1 << ADSC))
		;
		// save result
		up += ADC;
		// settle, discharge
		settle_delay();
		clear(touchpad_port, touchpad_transmitter_pin);
		// initiate conversion
		ADCSRA |= (1 << ADSC);
		// wait for completion
		while (ADCSRA & (1 << ADSC))
		;
		// save result
		down += ADC;
	}
	//
	// send framing
	//
	output(serial_direction, serial_pin_out);
	put_char(&serial_port, serial_pin_out, 1);
	char_delay();
	put_char(&serial_port, serial_pin_out, 2);
	char_delay();
	put_char(&serial_port, serial_pin_out, 3);
	char_delay();
	put_char(&serial_port, serial_pin_out, 4);
	//
	// send result
	//
	put_char(&serial_port, serial_pin_out, (up & 255));
	char_delay();
	put_char(&serial_port, serial_pin_out, ((up >> 8) & 255));
	char_delay();
	put_char(&serial_port, serial_pin_out, (down & 255));
	char_delay();
	put_char(&serial_port, serial_pin_out, ((down >> 8) & 255));
	char_delay();
	input(serial_direction, serial_pin_out);
}

void put_char(volatile unsigned char *port, unsigned char pin, char txchar) {
   //
   // send character in txchar on port pin
   //    assumes line driver (inverts bits)
	 //		 serial_pin_out must be enabled (configured as output) before sending
	 //	   and must be disabled (configured as input) after sending
   //
   // 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 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_string(volatile unsigned char *port, unsigned char pin, PGM_P str) {
   //
   // send character in txchar on port pin
   //    assumes line driver (inverts bits)
   //
   static char chr;
   static int index;
   index = 0;
   do {
      chr = pgm_read_byte(&(str[index]));
      put_char(&serial_port, serial_pin_out, chr);
      ++index;
      } while (chr != 0);
   }
