#include <iostream>
#include <cmath>
#include <fstream>

using namespace std;

// Dual Lead Compensator Values
const double currentErrorGain = 29.07;
const double prevErrorGain = -56.78;
const double prevPrevErrorGain = 27.71;
const double prevPumpSpeedGain = 1.238;
const double prevPrevPumpSpeedGain = -0.3829;

const double pumpSpeedUpperLimit = 50.0;     // [rev/sec]
const double pumpSpeedLowerLimit = -50.0;    // [rev/sec]

static double currentPressureError = 0.0;
static double prevPressureError = 0.0;
static double prevPrevPressureError = 0.0;

static double nextPumpSpeedCmd = 0.0;
static double prevPumpSpeedCmd = 0.0;
static double prevPrevPumpSpeedCmd = 0.0;


// Reference Governor Values
const double maxSetPointIncrement = 1.2;   // [decibar]
const double maxSetPointDecrement = -1.2;  // [decibar]

static double governedPressureSetPoint = 0.0; // [decibar]


void ReferenceGovernor(double desiredPressureSetPoint)
{
    // AUTHOR: Brian Ha (2018 Summer Intern)


    //
    double setPointDelta;   // [decibar]
    setPointDelta = desiredPressureSetPoint - governedPressureSetPoint;
    cout << "REF GOV:  setPointDelta = " << setPointDelta << "; ";


    //
    if (setPointDelta > maxSetPointIncrement)
    {
        setPointDelta = maxSetPointIncrement;
    }
    else if (setPointDelta < maxSetPointDecrement)
    {
        setPointDelta = maxSetPointDecrement;
    }

    //
    governedPressureSetPoint = governedPressureSetPoint + setPointDelta;

    if(abs(desiredPressureSetPoint - governedPressureSetPoint) <= 1.2)
    {

        governedPressureSetPoint = desiredPressureSetPoint;
    }

    cout << "governedPressureSetPoint = " << governedPressureSetPoint << "; ";

    //return governedPressureSetPoint;
}


double DepthController(double currentPlatformPressure)
{
    // AUTHOR: Brian Ha (2018 Summer Intern)

    // Calculate current pressure error
    currentPressureError = governedPressureSetPoint - currentPlatformPressure;
    cout << "ERROR = " << currentPressureError << "; ";


    // Calculate the next pump speed command.
    nextPumpSpeedCmd = (currentErrorGain * currentPressureError) + (prevErrorGain * prevPressureError) + (prevPrevErrorGain * prevPrevPressureError) + (prevPumpSpeedGain * prevPumpSpeedCmd) + (prevPrevPumpSpeedGain * prevPrevPumpSpeedCmd);


    // Limit pump speed command magnitude to 50 revs/sec.
    if (nextPumpSpeedCmd > pumpSpeedUpperLimit)
    {
        nextPumpSpeedCmd = pumpSpeedUpperLimit;
    }
    else if(nextPumpSpeedCmd < pumpSpeedLowerLimit)
    {
        nextPumpSpeedCmd = pumpSpeedLowerLimit;
    }

    // Deadband
    if (nextPumpSpeedCmd < 0.05 && nextPumpSpeedCmd > -0.05)
    {
        nextPumpSpeedCmd = 0.0;
    }

    // Set historical values for next function call
    prevPrevPumpSpeedCmd = prevPumpSpeedCmd;
    prevPrevPressureError = prevPressureError;
    prevPumpSpeedCmd = nextPumpSpeedCmd;
    prevPressureError = currentPressureError;


    // Convert from rev/sec to counts/sec
    //nextPumpSpeedCmd = nextPumpSpeedCmd * COUNTSperREV;


    // Return next pump speed command [counts/sec].
    return (nextPumpSpeedCmd);
}



int main() {
    double desiredPressureSetPoint;
    double currentPlatformPressure;

    ifstream infile("pressures.txt");
    ifstream infile2("references.txt");
    ofstream outfile("ctrl_signals.txt");

    while (infile >> currentPlatformPressure)
    {
        infile2 >> desiredPressureSetPoint;
        ReferenceGovernor(desiredPressureSetPoint);
        nextPumpSpeedCmd = DepthController(currentPlatformPressure);
        outfile << nextPumpSpeedCmd << endl;
        cout << "CONTROLLER: " << nextPumpSpeedCmd << " [revs/sec]" << endl;

    }
    outfile.close();


    return 0;
}