#define LED_RED 13
#define MOTOR_DIR_PIN 4
#define MOTOR_SPEED_PIN 5
#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3
#define COUNTER_PER_MM 10.
#define dt 50



/* ------ Vars ------ */
// Turns by default 3000mm
float finalOrder = 3000; // mm
float order = 0;
float previousOrder = 0;

// PID parameters
const float paramP = 2;
const float paramI = 0.012;
const float paramD = 0.01;

unsigned long time = 0;
unsigned long orderTime = 0;
float tpP = 0;
long counter = 0;

float error = 0;
float error2 = 0;
float error3 = 0;
float error4 = 0;

float integrate = 0;
float derivate = 0;

// Time to get the order to the final order
float period = 10000; // ms

char chanAState = 0;
char chanBState = 0;

int dir = 0;
int power;
bool disable = false;

// Type of curve followed for a new order
// 0 : step, 1 : ramp, 2 : smooth
int type = 1;

void setup() {
  pinMode(MOTOR_DIR_PIN, OUTPUT);
  pinMode(MOTOR_SPEED_PIN, OUTPUT);

  // Enable serial
  Serial.begin(9600);

  time = millis()+dt;

  // Interrupts for the encoder
  attachInterrupt(digitalPinToInterrupt(ENCODER_A_PIN), channelAChange, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_B_PIN), channelBChange, CHANGE);
  orderTime = millis();
}

void loop() {
  // Check if the user wrote someting in serial
  if(Serial.available() > 0) {
    float tmp = Serial.parseFloat();
    Serial.parseInt();
    if(tmp == 0) { // 0 is to disable the motor
      disable = true;
      analogWrite(MOTOR_SPEED_PIN, 0);
      return;
    }
    disable = false;
    // Store the order in finalOrder and the loop will decide if it is a ramp or something else
    previousOrder = order;
    finalOrder = tmp;
    orderTime = millis();
  }
  // Control the power to the motor every dt
  if(time < millis()) {
    controlPower();
    time = millis()+dt;
  }
}

void controlPower() {
  if(disable) {
    analogWrite(MOTOR_SPEED_PIN, 0);
    return;
  }

  if(type == 0) { // Step
    order = finalOrder;
  } else if(type == 1) { // Ramp
    if(order != finalOrder)
      order = min(1., (millis()-orderTime)/period)*(finalOrder-previousOrder)+previousOrder;
  } else { // Complex shape to limit acceleration
    tpP = min(1., (float)(millis()-orderTime)/period);
    order = previousOrder + (finalOrder - previousOrder) * (10*pow(tpP,3)-15*pow(tpP,4)+6*pow(tpP,5));
  }

  // Errors are moved 1 up, simple to use array
  error4 = error3;
  error3 = error2;
  error2 = error;

  error = (float)counter/COUNTER_PER_MM-(float)order;

  // Calculation of the integrate value capped at |10000| to limit oscillations
  integrate = max(-10000, min(10000, integrate+error*(float)dt));

  // Calculate the derivative over the last 3 error values to limit noise
  derivate = (error-error4)/(float)dt/3.;

  // Calculate the power with our PID
  power = error*paramP + integrate*paramI + derivate*paramD;

  // Direction of rotation of the motor
  if(power > 0) {
    digitalWrite(MOTOR_DIR_PIN, HIGH);
  } else {
    digitalWrite(MOTOR_DIR_PIN, LOW);
  }

  // Setting the power PWM
  analogWrite(MOTOR_SPEED_PIN, min(255, abs(power)));

  // Display compatible with serial graph
  Serial.print(min(255, abs(power)));
  Serial.print(" ");
  Serial.print(order);
  Serial.print(" ");
  Serial.print(tpP);
  Serial.print(" ");
  Serial.println((float)counter/COUNTER_PER_MM);
}

void channelAChange() {
  if(chanAState == chanBState) {
    dir = 1;
  } else {
    dir = -1;
  }
  counter += dir;

  chanAState = digitalRead(ENCODER_A_PIN);
}

void channelBChange() {
  if(chanAState == chanBState) {
    dir = -1;
  } else {
    dir = 1;
  }
  counter += dir;

  chanBState = digitalRead(ENCODER_B_PIN);
}