#include "heater.h"
#include "msp430x16x.h"  //register and I/O definitions
#include "dwarfcore.h"
#include "espdwarf.h"
#include "cpld.h"
#include "dwarfcore.h"
#include "espdwarf.h"
#include "parser.h"
#include "i2cnode.h"

#define abs(a) (((a)>=0)?(a):(-a))

void turnHeaterOff(Heater* h)
{
	h->state=heaterOff;
	TBCCR3=0;
	set16(opComplete,thermal);//Signal not sent yet so send it!
    signalEV();
 
}
void HeaterServoLoop(Heater* h)
{
int32 error=0;
int32	PWM=0;
	//if we are in closed loop mode calculate PWM (for P controller)
  if(h->state==heaterOn)
  {
//	h->currentTemperature=(h->tempSense1-h->tempSense2)*(h->heaterSlope_num/h->heaterSlope_den)+h->tempSense1;
		
//	error=h->tProgram.targetT-h->currentTemperature;
	error=((int32)h->tProgram.targetT-(int32)h->tempSense1);
	PWM=(h->tCalib.heating.P*error);
	//add in error to form intergral
  	h->sum = h->sum + error;
    //check it's limits
  
 	if (h->sum > H_SUM_LIMIT) h->sum = H_SUM_LIMIT;
  	else if ( h->sum < -H_SUM_LIMIT) h->sum = -H_SUM_LIMIT;
    //calc the integral portion
    PWM += h->sum / h->tCalib.heating.I;
    
    PWM=PWM/HPWMSCALE;
	
	if(PWM>90)PWM=90;
	if(PWM<0)PWM=0;
	h->heaterPWM=PWM;
	
	if(h->elapsedTics>=h->tProgram.dwell)
	{
	turnHeaterOff(h);
	set16(opComplete,thermal);//Signal not sent yet so send it!
    signalEV();
 	
	}

	

	else{
		h->elapsedTics+=1;
	TBCCR3=PWM;}
  }

}
//This function sets slope varibles based on set point temperature, it is called when a new heater command is given
void calculateSlope(Heater* h)
{
	if(h->tProgram.targetT<2457)
	{
	h->heaterSlope_den=2;
	h->heaterSlope_num=1;
	}
	else
	{
		h->heaterSlope_num=1000000;
		h->heaterSlope_den=690000-63*(h->tProgram.targetT);
		if((h->heaterSlope_num/h->heaterSlope_den>1)|(h->heaterSlope_num/h->heaterSlope_den<0))
		{
		h->heaterSlope_den=2;
		h->heaterSlope_num=1;
		}
	}
}
	
void initHeater(Heater* h)
{
	h->tProgram.dwell=0;
	h->tProgram.targetT=0;
	h->tProgram.errHoldoff=0;
	h->tProgram.minT=0;
	h->tProgram.maxT=0;

	h->tCalib.heating.P=5;
	h->tCalib.heating.I=2;
    h->tCalib.heating.D=0;
	h->tCalib.cooling.P=0;
	h->tCalib.cooling.I=0;
	h->tCalib.cooling.D=0;
	h->heaterSlope_den=2;
	h->heaterSlope_num=1;
	h->currentTemperature=0;	//maybe calculated temp including adjustment
	h->elapsedTics=0;
	h->heaterPWM=0;
	h->coolerPWM=0;
	h->tempSense1=0;
	h->tempSense2=0;
	h->fixedPWM=0;
	h->state=heaterOff;
	h->tempSenseSwitch=0;
	h->sum=0;	 		//sum of error for integral term
	TBCCR3=0;	
	set_DCORSENSOR(DCORTEMPEN);

}
void startHeater(Heater* h)
{
    h->tProgram.targetT=(int16)(((68*(int32)h->tProgram.targetT)/10000) + 451-41);
   
 	if(	h->tProgram.targetT>4095) 
	  h->tProgram.targetT = 4095;
//	calculateSlope(h);
	h->state=heaterOn;
	h->elapsedTics=0;

}