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

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

typedef struct _RawData {
 double SC_Range;
 double SC_UpperPressure;
 double SC_LowerPressure;
 double PC_RPM;
 double RC_Pressure;
} RawData;



void Controllers(RawData Measurements)
{
    static RangeEstimator Est(.5, 1e-6, .2); //These settings should be changeable yet persistent, i.e. in a reloadable config file. There are methods to set these outside of the constructor (SetVariances).
    static PID RangePID(.5,.02,0,-30,30);    //These settings should be changeable yet persistent, i.e. in a reloadable config file. There are methods to set these outside of the constructor. (SetCoeffs and SetCmdLimits)
    static PID DepthPID(.5,.02,0,-30,30);    //These settings should be changeable yet persistent, i.e. in a reloadable config file. There are methods to set these outside of the constructor. (SetCoeffs and SetCmdLimits)
    enum Modes {RANGE_CONTROL, DEPTH_CONTROL, POWER_CONTROL};
    Modes Mode = RANGE_CONTROL;
    double CmdValue;
    double RangeTarget = 40.0;  //This setting should be changeable from the command-line, and does not to be persisent
    double DepthTarget = 30.0;  //This setting should be changeable from the command-line, and does not to be persisent
    double z;

    z = Est.RLS_Range(Measurements.SC_Range, Measurements.SC_UpperPressure, Measurements.SC_LowerPressure);

    switch(Mode)
      {
       case RANGE_CONTROL:  //Execute PID loop to control position based upon laser rangefinder
	 {
          CmdValue = RangePID.CommandSignal(RangeTarget,z);
          std::cout << CmdValue << "   " << z << std::endl;
	  //Send "CmdValue" to Power Converter Controller over CAN bus using appropriate command.

	 break;
	 }
       case DEPTH_CONTROL:     //Execute PID loop to control position based upon laser rangefinder
	 {
          CmdValue = DepthPID.CommandSignal(DepthTarget,Measurements.RC_Pressure);
	  //Send "CmdValue" to Power Converter Controller over CAN bus using appropriate command.
	 break;
	 }
       case POWER_CONTROL:
	 {
	  //Call a function to do something really fancy...
	 break;
	 }
      }

}



#ifdef CONTROLLERS_TEST
//g++ Controllers.cpp PID.cpp RangeEstimator.cpp -D CONTROLLERS_TEST -o Controllers_Test
int main(int argc, char *argv[]) {
    float z;
    int Max = 40000;
    RawData Meas;
    double z_meas, Pu_meas, Pl_meas;


    while (Max--) {
        cin >> Meas.SC_Range >> Meas.SC_UpperPressure >> Meas.SC_LowerPressure;
	Controllers(Meas);
    }
}


#endif