#include "Arduino.h"
#include "McKenzie.h"


PIDd::PIDd (float Kp, float Ti, float Td, float t, float PVmin, float PVmax, float CVmin, float CVmax){
  this->Kp = Kp;
  this->Ti = Ti;
  this->Td = Td;
  this->t = t;
  this->PVmin = PVmin;
  this->PVmax = PVmax;
  this->CVmin = CVmin;
  this->CVmax = CVmax;
  antiWind = false;        
  //Tn1=micros();
}
  
void PIDd::Reset() {
  CO = 0;
  COn1 = 0;
  scancount = 0;
  En1 = 0;
  En2 = 0;
}

float PIDd::Output (float Error) {
  this->Error = Error;

  PVspan = PVmax - PVmin;
  En = Error / PVspan;  
        
  Tc = micros();
  deltaT = (float(Tc-Tn1))/1000000;
  if (deltaT > t) {            
    deltaE = En-En1;
          
    //if ((scancount <1) && (Ti != 0)) {
    //                Pterm = 0;
    //}
    //else {
    //   Pterm =  100*Kp*deltaE;
    //}

    Pterm =  100*Kp*deltaE;
     
    if (Ti == 0) {
      Iterm = 0;
    }
    else {
      Iterm = 100*Kp*(En*deltaT)/(60*Ti);
    }
      
    if (scancount < 2) {
      Dterm = 0;
      scancount++;
    }
    else {
      Dterm = 100*Kp*(60*Td)*(En-2*En1+En2)/deltaT;
    }         
         
    CO = COn1 + (Pterm + Iterm + Dterm);
      
    /*      
    if (antiWind) {
      Serial.println ("AntiWind");          
    }
    */
      
    if (CO <0) {
      if (!antiWind) {
        COn1 = CO;             
      } 
      else{
        COn1 = 0;
      }                      
      CO=0;
      antiWind = true;
    }
    
    else if (CO > 100) {
      if (!antiWind) {
        COn1 = CO;  
      }
      else {
      COn1 = 100;
      }
      CO=100;
      antiWind = true;
    }
    else {
      antiWind = false;                      
      COn1 = CO;
    }
            
    Tn1 = Tc;
    En2 = En1;
    En1 = En;
    //COn1 = CO;          
  }    

  //mapf(float x, float in_min, float in_max, float out_min, float out_max)
  //return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;  
  //CV = mapf(CO,0,100,CVmin,CVmax);
  CV = (CO - 0) * (CVmax - CVmin) / (100 - 0) + CVmin;

  return CV;
}