//
// hello.step.45.c
//
// step response hello-world
//    9600 baud FTDI interface
//
// Neil Gershenfeld
// 10/27/10
//
// (c) Massachusetts Institute of Technology 2010
// Permission granted for experimental and personal use;
// license for commercial sale available from MIT.
//

#include <avr/io.h>
#include <util/delay.h>
#include <stdint.h>
//#include <stdlib.h>

#define output(directions,pin) (directions |= pin) // set port direction for output
#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 100 // 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

#define serial_port PORTA
#define serial_direction DDRA
#define serial_pin_out (1 << PA1)
#define charge_port PORTA
#define charge_direction DDRA
#define charge_pin (1 << PA2)
#define button_port PORTA
#define button_pin PINA
#define button_bv (1 << PA7)

#define NUM_POINTS 6
#define INIT_TIME 1
#define doDelay() _delay_us(15);
#define doInitDelay() _delay_us(1);
#define NUM_MODELS 3
#define REPEATS 16 //should be 2^SHIFT
#define SHIFT 4

#define DEBUG 0

static uint16_t model[NUM_POINTS*NUM_MODELS];//3
static uint16_t points[NUM_POINTS];
static uint32_t pointSums[NUM_POINTS];
static uint32_t dists[NUM_MODELS];
static uint32_t vars[NUM_POINTS*NUM_MODELS];
static uint8_t buttonPressedPrev;
static uint8_t numTrained;

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 sendChar(char c){
	  put_char(&serial_port, serial_pin_out, c);
	  char_delay();
}

/**
 * n should be the span of j (not i!!!!!!!!!!!)
 */
uint8_t getInd(uint8_t i, uint8_t j, uint8_t n){
	return i * n + j;
}

uint32_t getDist(uint32_t a, uint32_t b){
	int32_t a2, b2;
	a2 = a;
	b2 = b;
	a2 = (a2 - b2) * (a2 - b2);
	return (uint32_t)a2;
}

void delayNum(uint8_t n){
	//doInitDelay();
	uint8_t j = 0;
	for(j = 0; j < n; ++j)
		doDelay();
}

void setAllPoints(){
	static unsigned char up_lo,up_hi,down_lo,down_hi;
	static uint32_t up, down;
	uint8_t i;
	clear(charge_port, charge_pin);
	for(i = 0; i < NUM_POINTS; i++){
	   settle_delay();
	   set(charge_port, charge_pin);
	   //doDelay();
	   delayNum(i);
	   ADCSRA |= (1 << ADSC);
	   while (ADCSRA & (1 << ADSC));

	   up_lo = ADCL;
	   up_hi = ADCH;

	   settle_delay();
	   clear(charge_port, charge_pin);
	   //doDelay();
	   delayNum(i);

		 ADCSRA |= (1 << ADSC);
		 while (ADCSRA & (1 << ADSC));

	     down_lo = ADCL;
	     down_hi = ADCH;

	     up = up_hi;
	     down = down_hi;

	     up = up << 8;
	     down = down << 8;

	     up += up_lo;
	     down += down_lo;

	     points[i] = (up + (1023-down)) >> 1;//average
	     //points[i] = up;
	}
}

void setPointsViaSums(){
	uint8_t i,j;
	for(i = 0; i < NUM_POINTS; i++)
		pointSums[i] = 0;
	for(j = 0; j < REPEATS; ++j){//mean
		setAllPoints();
		for(i = 0; i < NUM_POINTS; i++)
			pointSums[i] += points[i];
	}
	for(i = 0; i < NUM_POINTS; i++)
		points[i] = pointSums[i] >> SHIFT;
}

void setVars(){//must have set model means.  will use numTrained
	uint8_t i,j;
	for(i = 0; i < NUM_POINTS; ++i)
		vars[getInd(numTrained, i, NUM_POINTS)] = 0;
	for(i = 0; i < REPEATS; ++i){
		setAllPoints();
		for(j = 0; j < NUM_POINTS; ++j){
			vars[getInd(numTrained, j, NUM_POINTS)] += getDist(points[j], model[getInd(numTrained, j, NUM_POINTS)]);
		}
	}
	for(i = 0; i < NUM_POINTS; ++i)
		vars[getInd(numTrained, i, NUM_POINTS)] = vars[getInd(numTrained, i, NUM_POINTS)] >> SHIFT;
}

void train(){
	uint8_t i,j;
	if(numTrained < NUM_MODELS){
		setPointsViaSums();
		for(i = 0; i < NUM_POINTS; i++)//simplest thing for now
			model[getInd(numTrained, i, NUM_POINTS)] = points[i];
		setVars();
		++numTrained;
	}
}



void setDists(){
	uint32_t tempDist;
	setPointsViaSums();
	uint8_t i,j;
	for(i = 0; i < NUM_MODELS; ++i)
		dists[i] = 1;
	for(i = 0; i < numTrained; i++){
		tempDist = 0;
		for(j = 0; j < NUM_POINTS; ++j)
			tempDist += getDist(points[j], model[getInd(i,j,NUM_POINTS)]);
		dists[i] = tempDist;
	}
}

void setDistsVar(){
	uint32_t tempDists[NUM_POINTS];
	uint32_t tempDist;
	uint8_t i,j,k;
	for(i = 0; i < NUM_MODELS; ++i)//init
		dists[i] = 100000;

	for(i = 0; i < numTrained; ++i){
		for(j = 0; j < NUM_POINTS; ++j)
			tempDists[j] = 0;
		for(j = 0; j < REPEATS; ++j){
			setAllPoints();
			for(k = 0; k < NUM_POINTS; ++k)//sum SqDists, add variance as approximation to gaussian (laweoafsdf)
				tempDists[k] += getDist(points[k], model[getInd(i, k, NUM_POINTS)]) + vars[getInd(i, k, NUM_POINTS)];
		}
		for(j = 0; j < NUM_POINTS; ++j)//divide sqDist by var after the sum (avoid underflow)
			tempDists[j] /= vars[getInd(i, j, NUM_POINTS)];
		tempDist = 0;
		for(j = 0; j < NUM_POINTS; ++j)
			tempDist += tempDists[j];
		dists[i] = tempDist;//could shift, but that'd just lose resolution
	}
}

unsigned char getClosest(){
	setDists();
	uint32_t dist;
	dist = UINT32_MAX;
	char closest = 12;
	uint8_t i;
	for(i = 0; i < numTrained; i++){
		if(dists[i] < dist){
			dist = dists[i];
			closest = i;
		}
	}
	return closest;
}

uint8_t buttonPressed(){
	return !(button_pin & button_bv);
}

void send16(uint16_t n){
	sendChar(n >> 8);
	sendChar(n);
}

void send32(uint32_t n){
	sendChar(n >> 24);
	sendChar(n >> 16);
	sendChar(n >> 8);
	sendChar(n);
}

void sendData(){
	uint8_t i;
	for(i = 0; i < NUM_POINTS; i++){
		send16(points[i]);
	}
}

void sendModels(){
	uint8_t i,j;
	for(i = 0; i < numTrained; i++){
		for(j = 0; j < NUM_POINTS; ++j){
			send16(model[getInd(i,j,NUM_POINTS)]);
		}
	}
}

void sendDists(){
	uint8_t i;
	for(i = 0; i < NUM_MODELS; ++i)
	{
		send32(dists[i]);
	}
}

void debug(){
	  sendChar(getClosest());
	  sendData();
	  sendModels();
}

int main(void) {
	buttonPressedPrev = 0;
	numTrained = 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);
   clear(charge_port, charge_pin);
   output(charge_direction, charge_pin);

   button_port |= button_bv;//pull up resistor

   uint8_t i;
   for(i = 0; i < NUM_MODELS; ++i)
	   dists[i] = 1;

   //
   // init A/D
   //
   ADMUX = (0 << REFS1) | (0 << REFS0) // Vcc ref
      | (0 << MUX3) | (0 << MUX2) | (1 << MUX1) | (1 << MUX0); // PA3
   ADCSRA = (1 << ADEN) // enable
      | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // prescaler /128
   //
   // main loop
   //
   //train();//0 is always background
   while (1) {
	  if(buttonPressed()){
		  if(!buttonPressedPrev){
			  buttonPressedPrev = 1;
			  sendChar(17);
			  train();
		  }
	  } else {
		  buttonPressedPrev = 0;
		  sendChar(4);
		  if(DEBUG){
			  debug();
		  } else {
			  sendChar(numTrained);
			  //setDists();
			  setDistsVar();
			  sendDists();
		  }
	  }

   }
}