#include <PID_v1.h>

#define ACCELERATION_TARGET_SPEED 120   // PWM 
#define DECELERATION_TARGET1_SPEED 60   // PWM
#define DECELERATION_TARGET2_SPEED 0    // PWM

#define MOTOR_CONTROL_PIN 25              // pwm pin
#define MOTOR_ENCODER_FEEDBACK_PIN 34     // encoder feedback pin

double Kp = 2.0, Ki = 5.0, Kd = 1.0;      // PID parameters

double target = 60;         // set desired pwm speed
double input = 0;           // current speed
double output = 0;          // control output

// Create PID controller
PID motorPID(&input, &output, &target, Kp, Ki, Kd, DIRECT);
unsigned long lastUpdate = 0;
unsigned long lastUpdate1 = 0;

void setup() {
  Serial.begin(115200);

  pinMode(MOTOR_CONTROL_PIN, OUTPUT);
  pinMode(MOTOR_ENCODER_FEEDBACK_PIN, INPUT);

  motorPID.SetMode(AUTOMATIC);       // Initialize PID controller
  motorPID.SetOutputLimits(0, 255);  // Output range for motor speed
  lastUpdate = millis();
}

// double Kp = 5.0, Ki = 2.0, Kd = 0.5;
// 41636
// 40000


void loop() {
  if (millis() - lastUpdate1 > 100) {
    setAcceleration();
    lastUpdate1 = millis();
  }
}

void setAcceleration() {

  int input1 = analogRead(MOTOR_ENCODER_FEEDBACK_PIN);
  input = map(input1, 0, 4095, 0, 255);
  motorPID.Compute();
  int output1 = map(output, 0, 255, 0, target);
  analogWrite(MOTOR_CONTROL_PIN, output1);

  Serial.print(input);
  Serial.print(" | ");
  Serial.print(output);
  Serial.print(" | ");
  Serial.println(output1);

  if (output1 >= 60) {
    Serial.println("reached!");
    lastUpdate = millis() - lastUpdate;
    Serial.println(lastUpdate);
  }
}