#include "PID_AutoTune.h"
#include <stdlib.h>

unsigned long PID_ATune_millis()
{
 return 0;	
}

void PID_ATune_Cancel(PID_ATune *pid_atune)
{
  pid_atune->running = TUNE_FALSE;
}


void PID_ATune_GetKp(PID_ATune *pid_atune,float *Kp)
{
  if(Kp!=NULL)
    *Kp=(pid_atune->controlType==1?0.6*(pid_atune->Ku):0.4*(pid_atune->Ku));
}


void PID_ATune_GetKi(PID_ATune *pid_atune,float *Ki)
{
  if(Ki!=NULL)
   *Ki=(pid_atune->controlType==1?1.2*(pid_atune->Ku)/(pid_atune->Pu):0.48*(pid_atune->Ku)/(pid_atune->Pu));  // Ki = Kc/Ti
}



void PID_ATune_GetKd(PID_ATune *pid_atune,float *Kd)
{
  if(Kd!=NULL)
   *Kd=(pid_atune->controlType==1?0.075*(pid_atune->Ku)*(pid_atune->Pu):0);  //Kd = Kc * Td
}

void PID_ATune_SetOutputStep(PID_ATune *pid_atune,float *Step)
{
  if(Step!=NULL)
	pid_atune->oStep = *Step;
}

void PID_ATune_GetOutputStep(PID_ATune *pid_atune,float *Step)
{
  if(Step!=NULL)
    *Step=pid_atune->oStep;
}

void PID_ATune_SetControlType(PID_ATune *pid_atune,int Type) //0=PI, 1=PID
{
	pid_atune->controlType = Type;
}

int PID_ATune_GetControlType(PID_ATune *pid_atune)
{
  return pid_atune->controlType;
}
	
void PID_ATune_SetNoiseBand(PID_ATune *pid_atune,float *Band)
{
  if(Band!=NULL)
    pid_atune->noiseBand = *Band;
}

void PID_ATune_GetNoiseBand(PID_ATune *pid_atune,float *noiseBand)
{
  if(noiseBand!=NULL)
    *noiseBand=pid_atune->noiseBand;
}


void PID_ATune_SetLookbackSec(PID_ATune *pid_atune,int value)
{
    if(value<1) 
	  value = 1;
	
	if(value<25)
	{
	  pid_atune->nLookBack = value*4;
	  pid_atune->sampleTime = 250;
	}
	else
	{
	  pid_atune->nLookBack = value/4;
	  if((pid_atune->nLookBack)>=(MAX_INPUTS-1))
	    pid_atune->nLookBack=(MAX_INPUTS-2);
	  pid_atune->sampleTime = 4;
	}
}


int PID_ATune_GetLookbackSec(PID_ATune *pid_atune)
{  
  return (pid_atune->nLookBack)*(pid_atune->sampleTime);//1000;
} 

void PID_ATune_FinishUp(PID_ATune *pid_atune)
{
  *(pid_atune->output) = pid_atune->outputStart;
  pid_atune->Ku = 4*(2*(pid_atune->oStep))/(((pid_atune->absMax)-(pid_atune->absMin))*3.14159);
  pid_atune->Pu = (float)((pid_atune->peak1)-(pid_atune->peak2));
}


void PID_ATune_Init(PID_ATune *pid_atune,float *Input,unsigned short *Output)
{
	pid_atune->input = Input;    
	pid_atune->output = Output;  
	
	pid_atune->controlType =TYPE_PID;  
	pid_atune->noiseBand = NOISE_BAND;
	pid_atune->running = TUNE_FALSE;
	pid_atune->oStep = OSTEP; 
	PID_ATune_SetLookbackSec(pid_atune,LOOK_BACK_TIME);  
	pid_atune->lastTime = 0;	
}


int PID_ATune_Runtime(PID_ATune *pid_atune,unsigned long time_tick)
{
    unsigned long now;
    float refVal;
    int i;
    float val;
    float avgSeparation;
    
	pid_atune->justevaled=TUNE_FALSE;
	if((pid_atune->peakCount)>(MAX_PEAKS-1)&&(pid_atune->running))
	{
	  pid_atune->running = TUNE_FALSE;
	  PID_ATune_FinishUp(pid_atune);
	  return 1;
	}
	
    now = time_tick;	

	pid_atune->lastTime = now;
	
	
	
	refVal = *(pid_atune->input); 
	
	pid_atune->justevaled=TUNE_TURE;
	
	if(!(pid_atune->running))
	{ 
		pid_atune->peakType = 0;
		pid_atune->peakCount=0; 
		pid_atune->justchanged=TUNE_FALSE; 
		pid_atune->absMax=refVal;
		pid_atune->absMin=refVal;
		pid_atune->setpoint = refVal;
		pid_atune->running = TUNE_TURE;
		pid_atune->outputStart = *(pid_atune->output);
		*(pid_atune->output) = (pid_atune->outputStart)+(pid_atune->oStep);
	}
	else
	{  
		if(refVal>(pid_atune->absMax)) 
		   pid_atune->absMax=refVal;
		if(refVal<(pid_atune->absMin))
		  pid_atune->absMin=refVal;
	}
	

  if(refVal>((pid_atune->setpoint)+(pid_atune->noiseBand))) 
     *(pid_atune->output) = (pid_atune->outputStart)-(pid_atune->oStep);
  else if (refVal<((pid_atune->setpoint)-(pid_atune->noiseBand))) 
     *(pid_atune->output) = (pid_atune->outputStart)+(pid_atune->oStep);
	
	
 
  pid_atune->isMax=TUNE_TURE;
  pid_atune->isMin=TUNE_TURE;
 
  for(i=((pid_atune->nLookBack)-1);i>=0;i--)
  { 
    val = pid_atune->lastInputs[i];
    if(pid_atune->isMax) 
	   pid_atune->isMax = (refVal>val);
    if(pid_atune->isMin) 
	   pid_atune->isMin = (refVal<val);
    pid_atune->lastInputs[i+1]=pid_atune->lastInputs[i];
  }
  pid_atune->lastInputs[0] = refVal;
  
  
  
  if((pid_atune->nLookBack)<9)
  {  
	return 0;
  }
  
 
  if((pid_atune->isMax))
  {
    if((pid_atune->peakType)==0)
	  pid_atune->peakType=1;
    if((pid_atune->peakType)==-1)
    {
      pid_atune->peakType = 1;
      pid_atune->justchanged=TUNE_TURE;
      (pid_atune->peak2) = (pid_atune->peak1);
    }
    pid_atune->peak1 = now;
    pid_atune->peaks[(pid_atune->peakCount)]=refVal;
   
  }
  else if((pid_atune->isMin))
  {
    if((pid_atune->peakType)==0)
	  pid_atune->peakType=-1;
    if((pid_atune->peakType)==1)
    {
      pid_atune->peakType=-1;
      pid_atune->peakCount++;
      pid_atune->justchanged=TUNE_TURE;
    }
    
    if((pid_atune->peakCount)<MAX_PEAKS)
	  pid_atune->peaks[(pid_atune->peakCount)] = refVal;// 
  }
  
  
  if((pid_atune->justchanged)&&(pid_atune->peakCount>2))
  {  
	avgSeparation = (abs(pid_atune->peaks[(pid_atune->peakCount)-1]-(pid_atune->peaks[(pid_atune->peakCount)-2]))+abs(pid_atune->peaks[(pid_atune->peakCount)-2]-(pid_atune->peaks[(pid_atune->peakCount)-3])))/2;
   
    if(avgSeparation<0.05*((pid_atune->absMax)-(pid_atune->absMin)))
    {
	  PID_ATune_FinishUp(pid_atune);
      pid_atune->running= TUNE_FALSE;
	  return 1;	 
    }
  }
   pid_atune->justchanged=TUNE_FALSE;
  return 0;
}