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





// Operation:
//
//                                             ----   Proportional
//                                            |    |  Output
//                             ---------------| Kp |-----------------
//                            |               |    |                 |
//                            |                ----                  |
//Reference                   |                                     ---
//Input         ---           |           --------------  Integral | + | Control   -------
//     --------| + |  Control |          |      Ki      | Output   |   | Output   |       |
//             |   |----------|----------| ------------ |----------|+  |----------| Plant |--
//        -----| - |Difference|          |  1 - Z^(-1)  |          |   |          |       |  |
//       |      ---  (error)  |           --------------           | + |           -------   |
//       |                    |                                     ---                      |
//       | Measured           |         -------------------  Deriv   |                       |
//       | Outut              |        |                   | Output  |                       |
//       |                     --------| Kd * (1 - Z^(-1)) |---------                        |
//       |                             |                   |                                 |
//       |                              -------------------                                  |
//       |                                                                                   |
//       |                                                                                   |
//        -----------------------------------------------------------------------------------
//
//   controlOutput[n] = controlOutput[n-1]
//                    + controlHistory[n] * abcCoefficients[0]
//                    + controlHistory[n-1] * abcCoefficients[1]
//                    + controlHistory[n-2] * abcCoefficients[2]
//
//  where:
//   abcCoefficients[0] = Kp + Ki + Kd
//   abcCoefficients[1] = -(Kp + 2*Kd)
//   abcCoefficients[2] = Kd
//   controlHistory[n] = measuredOutput[n] - referenceInput[n]
//  where:
//   abcCoefficients, controlHistory, controlOutput, measuredOutput and controlReference
//   are all members of the data structure tPID.


#ifndef PID_H
#define PID_H



class PID {
public:
double controlOutput; 
private:
double controlHistory[3];   
double Coeffs[3];
double MaxCmd;
double MinCmd;

public:
    PID();
    PID(double Kp, double Ki, double Kd);
    PID(double Kp, double Ki, double Kd, double Min, double Max);
    void SetCoeffs(double Kp, double Ki, double Kd);
    void SetCmdLimits(double Min,double Max);
    double CommandSignal(double ReferenceInput,double Measurement);
};

#endif