#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;
	h->sum=0;
	TBCCR3=0;
	set16(opComplete,thermal);//Signal not sent yet so send it!
    signalEV();
 
}
void HeaterServoLoop(Heater* h)
{
int32 error=0;
int32	PWM=PWMSTART;
	//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;
h->temp=h->tProgram.targetT;
	error=((int32)h->tProgram.targetT-(int32)h->tempSense1);
	PWM +=(h->tCalib.heating.P*error);
	
//	if ((PWM < MAXPPWM) && (PWM > MAXNPWM))
	if ((PWM > IRESETPWM) || (PWM < -(IRESETPWM)))  h->sum = 0;
	//add in error to form intergral only if output 
	//not positivly saturated
  	else h->sum = h->sum + error;
    //check it's limits
    //changed to limit sum to h->intLimitN from -inlimit
 	if (h->sum > h->intLimitP) h->sum = h->intLimitP;
  	else if ( h->sum < h->intLimitN) h->sum = h->intLimitN;
    //calc the integral portion
    //if (h->tCalib.heating.I != 0)
     PWM += h->sum * h->tCalib.heating.I;
    
    PWM=PWM/HPWMSCALE;
	h->Pterm=(h->tCalib.heating.P*error)/HPWMSCALE;
    h->Iterm=(h->sum / h->tCalib.heating.I)/HPWMSCALE;
	if(PWM>MAXHPWM)PWM=MAXHPWM;
	if(PWM<MINHPWM)PWM=MINHPWM;
	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=80;
	h->tCalib.heating.I=16;
    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
	h->intLimitP=0;
	h->intLimitN = 0;
	TBCCR3=0;	

}
void startHeater(Heater* h)
{
h->temp=h->tProgram.targetT;
  //  h->tProgram.targetT=(int16)(((4096UL*68UL*(uint32)h->tProgram.targetT)/1000000Ul) + 451-41);
//convert the tT given from 0.01 C to counts
        h->tProgram.targetT=(int16)((int32)(3355)*(int32)h->tProgram.targetT/(int32)(8192));
 h->temp=h->tProgram.targetT;  
 h->intLimitP=(int32)(((int32)MAXPPWM)/h->tCalib.heating.I);
 h->intLimitN=(int32)(-((int32)MAXNPWM)/h->tCalib.heating.I);
 	if(	h->tProgram.targetT>4095) 
	  h->tProgram.targetT = 4095;
//	calculateSlope(h);
	h->state=heaterOn;
//	h->elapsedTics=0;

}