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

#include "heater.h"

static int PID;

static int32 PWM0, PWM1;

static unsigned int shiftreg;

/*
0 = 0000 0000 0000 0000 = 0x0000
1 = 0000 0000 0000 0001 = 0x0001
2 = 0000 0001 0000 0001 = 0x0101
3 = 0010 0001 0000 1000 = 0x2108
4 = 0001 0001 0001 0001 = 0x1111
5 = 0010 0100 1001 0010 = 0x2492
6 = 0100 1010 0100 1001 = 0x4A49
7 = 0101 0010 0101 0101 = 0x5255
8 = 0101 0101 0101 0101 = 0x5555
9 = 1010 1101 1010 1010 = 0xADAA
A = 1011 0101 1011 0110 = 0xB5B6
B = 1101 1011 0110 1101 = 0xDB6D
C = 1110 1110 1110 1110 = 0xEEEE
D = 1101 1110 1111 0111 = 0xDEF7
E = 1111 1110 1111 1110 = 0xFEFE
F = 1111 1111 1111 1110 = 0xFFFE
*/

static unsigned int shiftarray[FRACTIONS] = {0x0000,0x0001,0x0101,0x2108,0x1111,0x2492,0x4a49,0x5255,0x5555,0xadaa,0xb5b6,0xdb6d,0xeeee,0xdef7,0xfefe,0xfffe};

static int ditherCount = 1;



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)
  {
	
    if (--ditherCount <= 0)
     { 
          //update PID after 16 cycle
          //reset ditherCount to run again
            ditherCount = FRACTIONS;
		
   //	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 > 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); 
	  return;	
	}
	else{
	  h->elapsedTics+=1;
          //get the dither pattern
       shiftreg = shiftarray[PWM&0x0000000f];
         //setup the two dither values
       PWM0 = (PWM & 0xfffffff0) >> 2;
       PWM1 = PWM0 + 4;

	 //  old way  TBCCR3=PWM;
        }
     }  
     //if the LSB of shift reg is one then turn on at PID value
     if (shiftreg & LSBmask) TBCCR3 = PWM1;
     //if is is 0 then shut off
     else TBCCR3 = PWM0;
     //update for next time
     shiftreg >>= 1;

    
  }

}
//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;

}