#ifndef __PIDCONTROL_H
#define __PIDCONTROL_H
#include "DCMotor.h"

//global control variables
float angle_now = 0;
float angle_speed_now = 0;
float angle_sum = 0;
unsigned long time_now = 0;
float filter_spread = 0;
int voltage = 0;
float Kp = 0;
float Kd = 0;
float Ki = 0;
int pm_counter = 0;

//control action
void pid_control(float yaw, float pitch, float roll) {
  unsigned long time_prev = time_now;
  time_now = micros();
  
  //CALC POSITION
  float angle_prev = angle_now;
  //angle_now = 16.5 - (yaw); //yaw
  angle_now = roll+42.7;
  //filter
  angle_now = (1-filter_spread)*angle_now + filter_spread*angle_prev;
  
  //CALC SPEED
  float angle_speed_prev = angle_speed_now;
  angle_speed_now = ((1-filter_spread) * (angle_now-angle_prev) / (time_now-time_prev) * 1000000) + filter_spread * angle_speed_prev;
  
  //CALC INTEGRAL
  angle_sum += angle_now + (time_now-time_prev) * 100000; //should be 1000000 but is scaled down right now

  //check if it has gone over the zero angle
  //if ((angle_prev >= 0.0 && angle_now < 0.0) || (angle_prev < 0.0 && angle_now >= 0.0)) {
  //  pm_counter = 10;
  //  Serial.println("gone over zero");
  //}

  voltage = round(constrain(abs(angle_now*Kp + angle_speed_now*Kd + angle_sum*Ki),0,255));
  myMotor->setSpeed(voltage);
  if (angle_now > 0.0) {
    myMotor->run(BACKWARD);
  }
  else {
    myMotor->run(FORWARD);
  }
}

#endif //__PIDCONTROL_H