class SimplePID{
  private:
    float Kp, Kd, Ki, umax; // umax maximun PWM value [255]
    float prev_error; // Previous error
    float integral_error; // Integral error

  public:
    // Constructor
    // Init PID parameter Kp = 1, Ki = 0, Kd = 0 
    SimplePID() : Kp(1), Ki(0), Kd(0), umax(255), prev_error(0.0), integral_error(0.0){}

    // Set parameters
    void setParams(float Kp_in, float Ki_in, float Kd_in, float umax_in){
       Kp = Kp_in;
       Ki = Ki_in;
       Kd = Kd_in;
       umax = umax_in;
    }

    // Calculate PID control signal
    void controlSignal(int value, int target, float deltaTime, int &pwr, int &dir){

      // Calculate error
      float error = target - value;

      // Derivative
      float dedt = (error - prev_error)/deltaTime;
    
      // Integral
      float integral_error = integral_error + error * deltaTime;
    
      // Compute control signal
      float u = Kp*error + Kd*dedt + Ki*integral_error;

      // Motor power
      pwr = (int)fabs(u);
      if (pwr > umax){
        pwr = umax;
      }
    
      dir = 1;
      if (u < 0){
        dir = 0;
      }
    }
};

/*
 *  ---- MOTOR DISTRUBUTION ----
 *    MOTOR 1 = Front Left
 *    MOTOR 2 = Rear Left
 *    Motor 3 = Front Right
 *    Motor 4 = Rear Right
 */
//============================================================================
// PINS DEFINITIONS

//==================================
// MOTOR 3
#define MOTOR3_A 27   //AIN1-2
#define MOTOR3_B 28   //AIN2-2
#define PWM3 8        //PWMA-2

// ENCODER 3
#define ENC3_A 4      // INTERRUPT
#define ENC3_B 0      // DIGITAL PIN
//==================================

//==================================
// MOTOR 4
#define MOTOR4_A 10   //BIN1-2
#define MOTOR4_B 11   //BIN2-2
#define PWM4 15       //PWMB-2

// ENCODER 4
#define ENC4_A 9      // INTERRUPT
#define ENC4_B 2      // DIGITAL PIN
//==================================

//==================================
// MOTOR 1
#define MOTOR1_A 18   //AIN1-1
#define MOTOR1_B 17   //AIN2-1
#define PWM1 16       //PWMA-1

// ENCODER 1
#define ENC1_A 14     // INTERRUPT
#define ENC1_B 3      // DIGITAL PIN
//==================================

//==================================
// MOTOR 2
#define MOTOR2_A 19   //BIN1-1
#define MOTOR2_B 22   //BIN2-1
#define PWM2 23       //PWMB-1

// ENCODER 2
#define ENC2_A 5      // INTERRUPT
#define ENC2_B 1      // DIGITAL PIN
//==================================

//==================================
#define STBY_1 6 // Stanby driver 1 (HIGH to enable the driver)
#define STBY_2 7 // Stanbt driver 2 (HIGH to enable the driver)
//==================================
//============================================================================

//============================================================================
// Motors array
#define NMOTORS 4 // Numbers of motors

// Control pinss A array - Motors
const int motors_a[] = {MOTOR1_A, MOTOR2_A, MOTOR3_A, MOTOR4_A};
// Control pinss B array - Motors
const int motors_b[] = {MOTOR1_B, MOTOR2_B, MOTOR3_B, MOTOR4_B}; 
// PWM PINs
const int pwm[] = {PWM1, PWM2, PWM3, PWM4};
//============================================================================

// Create Class instance
SimplePID pid[NMOTORS];

//============================================================================
// ENCCODERs

// Encoders PINs array
const int enc_a[] = {ENC1_A, ENC2_A, ENC3_A, ENC4_A};
const int enc_b[] = {ENC1_B, ENC2_B, ENC3_B, ENC4_B};

// Encoders position array
volatile int enc_pose[] = {0, 0, 0, 0}; // Init on zero

//============================================================================

//============================================================================
// RPM
#define PULSES 12 
// The encoder registers 48 pulses in total 
// for both sensors, high and low per rotation.
// But because only the high pulse of one of the sensors is recorded, 
// the number of pulses recorded per rotation is 12

#define WHEEL_CIRCUMFERENCE 2.0404// unit in meters

#define GEAR_RATIO 107.5 // Gearbox: 107.5:1;
float total_pulses = PULSES * GEAR_RATIO; // Pulses per turn
float pulsesPerMeter = total_pulses * WHEEL_CIRCUMFERENCE;
//============================================================================

// Every 1000ms
#define SAMPLE_TIME 1000
long previousMillis = 0;
long currentMillis = 0;


//==================================



long currentTime = 0;
long previousTime = 0;
float previousError = 0;
float integralError = 0;
float deltaTime = 0;


int error = 0;

float target_f[] = {0.0, 0.0, 0.0, 0.0};
long target[] = {0, 0, 0, 0};


// TODO
// fix rotation per seconds
// implement Serial comunication
float velocity[4] = {0.0, 0.0, 0.0, 0.0};
int velocity_int[4] = {0, 0, 0, 0};

void setTarget(float t, float deltat){
  float positionChange[4] = {0.0, 0.0, 0.0, 0.0};

  //t = fmod(t, 0.001); //time in seconds
  //Serial.print("[DEBUG] Velocity: ");
  //Serial.println(velocity[0]);


  for (int k = 0; k < 4; k++){ 
    positionChange[k] = velocity[k] * deltat * pulsesPerMeter;
  }
//  else if (t < 8){
//    // BACKWARD
//    velocity[0] = -0.8;
//    velocity[1] = -0.8;
//    velocity[2] = -0.8;
//    velocity[3] = -0.8;
//    for (int k = 0; k < 4; k++){
//      positionChange[k] = velocity[k] * deltat * pulsesPerMeter;
//    }
//  }
//  else if (t < 12){
//    // LEFT
//    velocity[0] = -0.8;
//    velocity[1] = 0.8;
//    velocity[2] = -0.8;
//    velocity[3] = 0.8;
//    for (int k = 0; k < 4; k++){
//      positionChange[k] = velocity[k] * deltat * pulsesPerMeter;
//    }
//  }
//  else if (t < 16){
//    // RIGHT
//    velocity[0] = 0.8;
//    velocity[1] = -0.8;
//    velocity[2] = 0.8;
//    velocity[3] = -0.8;
//    for (int k = 0; k < 4; k++){
//      positionChange[k] = velocity[k] * deltat * pulsesPerMeter;
//    }
//  }


  //Serial.print("[DEBUG] PositionChange: ");
  //Serial.println(positionChange[0]);

  for (int k = 0; k < 4; k++){
    target[k] = (target[k] + positionChange[k]);
  }

  //Serial.println(positionChange[0]);

//  for (int k = 0; k < 4; k++){
//    enc_pose[k] = 0;
//  }
  
}

void setup() {

  Serial.begin(9600);

  for (int k = 0; k < NMOTORS; k++){
    // motors pin mode
    pinMode(motors_a[k], OUTPUT);
    pinMode(motors_b[k], OUTPUT);
    pinMode(pwm[k], OUTPUT);

    // Encoders pin mode
    pinMode(enc_a[k], INPUT);
    pinMode(enc_b[k], INPUT);

    pid[k].setParams(1, 0, 0, 255);
  }

  // Interrupts definitions
  attachInterrupt(digitalPinToInterrupt(ENC1_A), triggerEncoder<0>, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC2_A), triggerEncoder<1>, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC3_A), triggerEncoder<2>, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC4_A), triggerEncoder<3>, RISING);

  //==================================
  // ---- STANDBY ----
  pinMode(STBY_1, OUTPUT);
  pinMode(STBY_2, OUTPUT);
  digitalWrite(STBY_1, HIGH); // HIGH to enable the driver
  digitalWrite(STBY_2, HIGH); // HIGH to enable the driver
  //==================================

  delay(5000);
}

long prevT = 0;



void loop() {

  while (Serial.available() > 0){
    velocity_int[0] = Serial.parseInt();
    velocity_int[1] = Serial.parseInt();
    velocity_int[2] = Serial.parseInt();
    velocity_int[3] = Serial.parseInt();
    velocity[0] = (float)velocity_int[0]/100;
    velocity[1] = (float)velocity_int[1]/100;
    velocity[2] = (float)velocity_int[2]/100;
    velocity[3] = (float)velocity_int[3]/100;
//    Serial.print(velocity[0]);
//    Serial.print("|");
//    Serial.print(velocity[1]);
//    Serial.print("|");
//    Serial.print(velocity[2]);
//    Serial.print("|");
//    Serial.println(velocity[3]);
  }

  // get Current time
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/(1.0e6);
  prevT = currT;

  setTarget(currT/1.0e6, deltaT); // in milli


  int pos[NMOTORS];
  noInterrupts();
  for(int k = 0; k < NMOTORS; k++){
    pos[k] = enc_pose[k];
  }
  interrupts();

  for (int k = 0; k < NMOTORS; k++){
    int pwr, dir;

    pid[k].controlSignal(pos[k], target[k], deltaT, pwr, dir);
    runMotor(motors_a[k], motors_b[k], pwm[k], pwr, dir);
  }

  // For debbuging
   for (int k = 0; k < NMOTORS; k++){
     Serial.print(target[k]);
     Serial.print(" ");
     Serial.print(pos[k]);
     Serial.print(" ");
   }
   Serial.println();
}

void runMotor(int input_1, int input_2, int pin_pwm, int input_speed, bool dir){
/*
 * runMotor(PIN_1, PIM_2, PIN_PWM, PIN_PWM[0 to 255], dir[0 (BACKWARD) or 1 (FORWARD)])
 */
   
  analogWrite(pin_pwm, input_speed);
  
  if (dir == true){
    digitalWrite(input_1, HIGH);
    digitalWrite(input_2, LOW);
  }
  else{
    digitalWrite(input_1, LOW);
    digitalWrite(input_2, HIGH);    
  }
}

template <int j>
void triggerEncoder(){
  if (digitalRead(enc_b[j]) == LOW){ // Forward
    enc_pose[j] ++;
  }else{ // Backward
    enc_pose[j] --;
  }
  
  // for debugging
//  Serial.print("ENCODER ");
//  Serial.print(j+1);
//  Serial.println(" triggered");
}