#include <PID_v1.h>
#include <ESP32Encoder.h>

int signum(double x){
  return (x > 0) ? 1 : ((x < 0) ? -1 : 0);
}

class Motor{
  public:
    Motor(int _forward, int _reverse, int _enable, int _enc1, int _enc2, int encMode=0){
      /*
      Takes input control pins for h-bridge, enable pin for pwm control
      and 2 encoder pins, and an encoder mode (0 = quadrature, 1 = 2x mode, 2 = 1x mode)
      */
      forward = _forward;
      reverse = _reverse;
      enable = _enable;
      
      pinMode(forward, OUTPUT);
      pinMode(reverse, OUTPUT);
      pinMode(enable, OUTPUT);


      ledcAttach(enable, 5000, 10);

      pinMode(_enc1, INPUT);
      pinMode(_enc2, INPUT);

      motorPID = new PID(&input, &out, &setpoint, 0,0,0,0);
      motorPID->SetOutputLimits(-1.0, 1.0);

      ESP32Encoder::useInternalWeakPullResistors = puType::down;
      //encoder = ESP32Encoder::ESP32Encoder();
      switch(encMode){
        case 0:
          encoder.attachFullQuad(_enc1, _enc2);
        break;
        case 1:
          encoder.attachHalfQuad(_enc1, _enc2);
        break;
        case 2:
          encoder.attachSingleEdge(_enc1, _enc2);
        break;
        default:
          Serial.println("Encoder mode out of range, defaulted to full quad");
        break;
      }
    }

    void setPID_Enabled(bool enable){
      PID_Enabled = enable;
    }

    void configPIDF(double kP, double kI, double kD, double _kF=0){
      motorPID->SetTunings(kP, kI, kD);

      kF = _kF;
    }

    void setPIDF(double _setpoint){
      setpoint = _setpoint;
    }

    void setPercentOutput(double percent){
      output = percent;
    }

    void setBrake(){
      // Set h Bridge to brake
      output = 0;
    }

    void setBrakeMode(bool mode){
      // if true brake, else coast
      brakeMode = mode;
    }

    void updatePID(){
      input = analogRead(34);//encoder.getCount();
      bool outputUpdated = motorPID->Compute();
      if(PID_Enabled){
        if (outputUpdated){
          output = out + signum(setpoint-input) * kF;
        }
      }
    }

    void writeOutput(){
      int state = 0;
      if (output > 0){
        //Set Hbridge to forward
        state = 1;
      }
      else{
        //Set Hbridge to reverse
        state = 2;
        output*=-1;
      }

      if ( output < 0.05){
        analogWrite(enable, 0);
        if (brakeMode){
          state = -1;
        }
        else{
          state = 0;
        }
        Serial.println("brake");
      }

      switch(state){
        case 1:
          digitalWrite(forward, HIGH);
          digitalWrite(reverse, LOW);
          analogWrite(enable, 1023*output);
        break;

        case 2:
          digitalWrite(forward, LOW);
          digitalWrite(reverse, HIGH);
          analogWrite(enable, 1023*output);
        break;

        case -1:
          digitalWrite(forward, HIGH);
          digitalWrite(reverse, HIGH);
          analogWrite(enable, 0);
        break;

        default:
          digitalWrite(forward, LOW);
          digitalWrite(reverse, LOW);
          analogWrite(enable, 0);
        break;
      }
    }

    void debugInfo(){
      Serial.println("********Motor Outputs*********");
      Serial.print("Encoder Position: ");
      Serial.println(encoder.getCount());
      Serial.print("Output: ");
      Serial.println(output);
    }

  private:
    PID *motorPID;
    
    ESP32Encoder encoder;

    double output = 0;

    double input = 0;
    double out = 0;
    double setpoint = 0;
    double kF = 0;

    int forward; 
    int reverse; 
    int enable;

    bool brakeMode = true;
    bool PID_Enabled = false;
}

#define PendulumLength 24;
//        Forward Reverse, Enable, Enc1, Enc2, mode
m = Motor(33, 25, 32, 34, 35, 0);

ESP32Encoder enc; 

double start;

void setup() {
  analogWriteResolution(32, 8);

  // may need to change pin 2 (blinks during startup)
  //ESP32Encoder::useInternalWeakPullResistors = puType::up;
  enc.attachFullQuad(34, 35);
  enc.useInternalWeakPullResistors = puType::up;
  
  // put your setup code here, to run once:
  Serial.begin(115200);
  //m = Motor(33, 25, 32, 34, 35, 0);
  start = millis();
}

void loop() {
  
  m.debugInfo();
  //Serial.print("Ellapsed Time: ");
  //Serial.println(millis()-start);
  
  if((millis()-start)<3000)
    m.setPercentOutput(1);
  else if((millis()-start)<5000)
    m.setPercentOutput(0);
  else if((millis()-start)<7000)
    m.setPercentOutput(-1);
  else
    m.setPercentOutput(0);
    
  m.writeOutput();
}