//
// hello.HC-SR04.c
//
// HC-SR04 sonar hello-world
//    9600 baud FTDI interface
//
// Neil Gershenfeld 11/15/15
// (c) Massachusetts Institute of Technology 2015
//
// 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.
//

#include <avr/io.h>
#include <util/delay.h>
//adition from node
#include <avr/pgmspace.h>
#include <string.h>
#include <inttypes.h>
#include <stdbool.h>

#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 char_delay() _delay_ms(10) // char delay
//node
#define serial_port PORTB
#define serial_direction DDRB
#define serial_pins PINB
#define serial_pin_in (1 << PB3)
#define serial_pin_out (1 << PB4)
//sonar
#define trigger_port PORTB
#define trigger_direction DDRB
#define trigger_pin (1 << PB2)
#define echo_pins PINB
#define echo_direction DDRB
#define echo_pin (1 << PB1)
//definir LED viene del código node
#define led_port PORTB
#define led_direction DDRB
#define led_pin (1 << PB0)
#define led_delay() _delay_ms(100) // LED flash delay

#define node_id '1'
#define dumbbell_signal 'd'
#define timeout 255
//define value

uint16_t filt = 0;
float eps = 0.1;
static uint16_t loop_counter, exercise;
//from node
void get_char(volatile unsigned char *pins, unsigned char pin, char *rxbyte);
void put_char(volatile unsigned char *port, unsigned char pin, char txchar);
void put_string(volatile unsigned char *port, unsigned char pin, PGM_P str);
void squats(void);
uint16_t measure_distance(void);

int main(void) {
	static char chr;
	
	//
	// main
	//
	// 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);
	input(serial_direction,serial_pin_out);
	clear(trigger_port,trigger_pin);
	output(trigger_direction,trigger_pin);
	set(led_port, led_pin);
	output(led_direction, led_pin);
	//
	// start counter
	//
	TCCR0B |= (1 << CS00); // prescale /1
	//
	// main loop
	//
	while (1) {
		//from pad	
		get_char(&serial_pins, serial_pin_in, &chr);
		_delay_ms(100);
		if (chr == dumbbell_signal){
			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);//
		}
		//sense and filter 200mm
		while(chr == dumbbell_signal){
			squats();
			if (exercise == 5){
				output(serial_direction, serial_pin_out);//
				static const char message_1[] PROGMEM = "Good Job!";
     			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);//
				exercise = 0;
				break;
			}
		}
		//
		// delay before next cycle
		//
		_delay_ms(10);
	}
}
void squats(void){
	for(int j=0; j<10; j++){
		filt = (1-eps)*filt+eps*measure_distance();
	}	      
	if(filt < 9280){
		//si está detectando
		loop_counter++;
		if(loop_counter == 1){
			exercise++;	
			clear(led_port, led_pin);
			_delay_ms(100);
			set(led_port, led_pin);
		}
	}else{
		//no esta detectando
		loop_counter = 0;
	}
}
uint16_t measure_distance(void){
	static unsigned char high,low;
	uint16_t value = 0;
	// trigger pulse
	//
	set(trigger_port,trigger_pin);
	_delay_us(10);
	clear(trigger_port,trigger_pin);  
	//
	// wait for echo rising edge
	//
	high = 0;
	TCNT0 = 0;
	TIFR |= (1 << TOV0);
	while (1) {
		if ((echo_pins & echo_pin) != 0) // check for rising edge
			break;
		if ((TIFR & (1 << TOV0)) != 0) { // check for counter overflow
			high += 1;
			if (high == timeout)
				break;
			TIFR |= (1 << TOV0);
		}
	}
	//
	// rising edge found, wait for falling edge
	//
	high = 0;
	TCNT0 = 0;
	TIFR |= (1 << TOV0);
	while (1) {
		if ((echo_pins & echo_pin) == 0) { // check for falling edge
			low = TCNT0;
			break;
		}
		if ((TIFR & (1 << TOV0)) != 0) { // check for counter overflow
			high += 1;
			if (high == timeout)
				break;
			TIFR |= (1 << TOV0);
		}
	}	
	value = (256*high + low);
	return value;
}

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