//////////////////////////////////////////////////////////////////////////////
// Copyright 2018 MBARI.                                                    //
// Monterey Bay Aquarium Research Institute Proprietary Information.        //
// All rights reserved.                                                     //
//////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <limits>
#include "PID.hpp"

PID::PID() {
controlOutput = 0.0;
controlHistory[0] = controlHistory[1] = controlHistory[2] = 0.0;
this->SetCoeffs(0,0,0);
this->SetCmdLimits(std::numeric_limits<double>::min(),std::numeric_limits<double>::max());
}

PID::PID(double Kp, double Ki, double Kd) {
controlHistory[0] = controlHistory[1] = controlHistory[2] = 0.0;
controlOutput = 0.0;
this->SetCoeffs(Kp,Ki,Kd);
this->SetCmdLimits(-std::numeric_limits<double>::max(),std::numeric_limits<double>::max());
}

PID::PID(double Kp, double Ki, double Kd,double Min, double Max) {
controlHistory[0] = controlHistory[1] = controlHistory[2] = 0.0;
controlOutput = 0.0;
this->SetCoeffs(Kp,Ki,Kd);
this->SetCmdLimits(Min,Max);
}

void PID::SetCoeffs(double Kp, double Ki, double Kd)
{
   Coeffs[0] = Kp + Ki + Kd;
   Coeffs[1] = -(Kp + 2*Kd);
   Coeffs[2] = Kd;
}

void PID::SetCmdLimits(double Min,double Max)
{
MinCmd = Min;
MaxCmd = Max;
}


double PID::CommandSignal(double ReferenceInput,double Measurement)
{
controlHistory[2] = controlHistory[1];
controlHistory[1] = controlHistory[0];
controlHistory[0] = Measurement-ReferenceInput;

controlOutput += Coeffs[0]*controlHistory[0] + Coeffs[1]*controlHistory[1] + Coeffs[2]*controlHistory[2];

if(controlOutput >= MaxCmd)
	controlOutput = MaxCmd;
if(controlOutput <= MinCmd)
	controlOutput = MinCmd;

return controlOutput;
}



#ifdef TEST_PID
//g++ PID.cpp -lm -DTEST_PID -o PID_Test

int main(int argc, char *argv[]) {
    int Iter = 10;
    PID PosPID(.5, 0, 0);
    double cmd;
    
    while (Iter--) {
        cmd = PosPID.CommandSignal(5,(double) Iter);
        std::cout << Iter << "  " << cmd   <<  "   " <<  PosPID.controlOutput  << std::endl;
    }
}

#endif