#include <Mega32.h>
#include <delay.h>

//Bruce definitions
#define begin {
#define end   }         

// Suggestions taken from 
// http://newton.ex.ac.uk/teaching/CDHW/Feedback/ControlTypes.html#FbkTypes
// and 
// http://atmel.com/dyn/resources/prod_documents/doc2558.pdf

#define Vref 3.3

// Defs for PID. Adjusting Kc will adjust how fast we change the temperature, Pc will adjust the oscillation and 
#define Kc 24 // Critical gain
#define Hot 0x01
#define Cold 0x02
#define Disable 0x04
#define AVG_N 8

int temp_prev; // Previous temperature
int temp_outside; // External temperature, sampled infrequently
int temp_desired; // Temperature we want to set to
int temp_now; // Temperature in the box
int temp_array[AVG_N]; // Array of previous temperatures, used to average out
float temp_avg;
float e_prev; // Last error term, used for derivitive in PID, also must be reset when we change desired temp
float e_next;

// NOTE THIS CODE ASSUMES THAT THE FUNCTION CALLING IT WILL MODIFY ADMUX TO READ THE APPROPRIATE THERMISTOR
int taketemp(void) 
begin  
   unsigned int temperature, Ain, temp_sum, m, n;
   float tempfloat, op_gain, vref;
   op_gain = 2.02/.72; //Empirically Calculated
   vref = 1.162; // Empirically Calculated
   // for 10 bit   AinLow = (int)ADCL;
   // 8 bit
   Ain = (int) (ADCH/op_gain) ; 
  // Ain = Ain*(Vref/op_gain); // 3.3V scale
   for (m = AVG_N-1; m > 0; m--)
   begin
        temp_array[m] = temp_array[m-1];
   end
   temp_array[0] = Ain;
   temp_sum = 0;
   n = 1;
   for (m = 0; m < AVG_N; m++)
   begin
        if (temp_array[m] != 0) 
        begin
        temp_sum += temp_array[m];
        n++;
        end
   end
   temp_avg = ((float) temp_sum) / ((float) n);
   
   tempfloat = (((float) temp_avg)/((float) 256))*((float) 330)*(vref);
   temperature = ((int) tempfloat);
   ADCSR.6 = 1; //next conversion
   //10mV per degree fahrenheit * 2 multiplier = 20mV / degree fahrenheit
   return temperature;
end  

// PID method
float PID(void)
begin
	float u; // control signal
	float Kp, Kd, D; // PID constants
	float T; // t = n*T, discrete time step T	
	Kp = Kc;
	D = .1; // Damping constant, increase to decrease ringing, decrease to increase speed
	T = .05; 
	Kd = Kc*D/T;
  //Kd = 0; // Determine ringing first
    e_prev = e_next;
	e_next = (float) (temp_desired - temp_now);
	u = Kp*e_next + Kd*(e_next - e_prev);
	return u;
end

int dutyPWM(float u) 
begin
     int i;
     if (u >= 0) i = (int) u;
     if (u < 0) i = (int) (-u);
     if (i > 255) i = 255;
     return i;
end

void resetError(void)
begin
     OCR0 = 0x00;
	temp_prev = 0;
	e_prev = 0;
	e_next = 0;
	temp_desired = temp_now;
end

void initTemp(void)
begin
    int m;
     ADMUX = 0b01100000;   
     //enable ADC and set prescaler to 1/128*16MHz=125,000
     //and clear interupt enable
     //and start a conversion
     ADCSR = 0b11000111;   
	// Fast PWM  mode, turn on PWM but set to 0 duty cycle
	TCCR0 = 0b01101010 ;
	temp_outside = 74;
	temp_now = 74;
	for (m = 0; m < AVG_N; m++)
	begin
	    temp_array[m] = 0;
	end 
	resetError();
end

char convertBin(int i)
begin
	char c = 0x00;
 	if (i - 128 >= 0)
 	begin
 		c = c ^ 0x80;
 		i = i - 128;
 	end
 	if (i - 64 >= 0) 
 	begin
 		c = c ^ 0x40;
 		i = i - 64;
 	end
 	if (i - 32 >= 0)
 	begin
 		c = c ^ 0x20;
 		i = i - 32;
 	end
 	if (i - 16 >= 0) 
 	begin
 		c = c ^ 0x10;
 		i = i - 16;
 	end
 	if (i - 8 >= 0) 
 	begin
 		c = c ^ 0x08;
 		i = i - 8;
 	end
 	if (i - 4 >= 0)
 	begin 
 		c = c ^ 0x04;
 		i = i - 4;
 	end
 	if (i - 2 >= 0) 
 	begin
 		c = c ^ 0x02;
 		i = i - 2;
 	end
 	if (i - 1 >= 0)
 	begin 
 		c = c ^ 0x01;
 		i = i - 1;
 	end
 	return c;
end

void updateP(void)
begin
     float u;
     char c;
     u = PID();
     if (temp_desired < temp_now)
     begin
          if(PORTB.0 == 1) PORTB.0 = 0; // Turn off hot
          if(PORTB.1 == 0) PORTB.1 = 1; // Turn on cold
          if(PORTB.2 == 1) PORTB.2 = 0; // Turn off disable 
          c = convertBin(dutyPWM(u));
          OCR0 = c;
     end
     else if (temp_desired > temp_now)
     begin
          if(PORTB.0 == 0) PORTB.0 = 1; //turn on hot
          if(PORTB.1 == 1) PORTB.1 = 0; //Turn off cold
          if(PORTB.2 == 1) PORTB.2 = 0; //Turn off disable
          c = convertBin(dutyPWM(u));
          OCR0 = c;
     end
     else
     begin
          OCR0 = 0x00;
          PORTB.0 = 0;
          PORTB.1 = 0;
          PORTB.2 = 1;
     end
end
// 
// void main(void)
// begin
// 	int i, j;
//   initTemp();
//   DDRB = DDRB | 0x0F;
// 	temp_desired = 82;
// 	temp_now = taketemp();
//   PORTC = ~convertBin(temp_now);
//   i = 0;
//   j = 0;
// 	while(1)
// 	begin
// 		i = i+1;
// 		if (i == 1000)
// 		begin
// 			j = j+1;
// 			if (j == 100)
// 			begin
// 				if (ADCSR.6 == 0) 
// 				begin
// 					temp_prev = temp_now;
// 					temp_now = taketemp();
// 				end
// 				updateP();
// 				PORTC = ~convertBin(temp_now);
// 				//PORTC = ~PORTC;
// 				j = 0;
// 			end
// 			i = 0;
// 		end
// 	end
// end