// https://wokwi.com/projects/380882887246419969 
// for https://forum.arduino.cc/t/implementing-pid-control-using-timerone-library/1179916/29

#include <TimerOne.h>

const int encoderPinA = 2;
const int pwmPin = 9;   // Motor PWM
const int dirPin = 51;  // Motor DIR
const int maxMotorRPM = 520;  // Maximum RPM of your motor

float simSpeed =0;
float counter = 0;  // Counter variable for counting pulses
int RPM1, currentRPM;

unsigned long currentTime, previousTime;
double elapsedTime;

//receiving serial data(<x,y>format) from esp32 to mega - variables
const byte numChars = 32;
char receivedChars[numChars];
boolean newData = false;
int xValue = 0;
int yValue = 0;

double kp = 0.001;   // Proportional gain
double ki = 0.6;    // Integral gain
double kd = 0.000082;  // Derivative gain

double error;
double lastError = 0;  // Initialize lastError to 0
double input, output;
int setPoint;
int mappedsetPoint = 0;
double cumError = 0;  // Initialize cumError to 0
double rateError;

void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);
  pinMode(encoderPinA, INPUT_PULLUP);
  pinMode(pwmPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(encoderPinA), encoder, RISING);
  Timer1.initialize(250000);           // Set timer interrupt interval to 0.25s
  Timer1.attachInterrupt(computerpm);  // Attach timer interrupt service routine
  digitalWrite(dirPin, LOW);  // Set direction
}

void loop() {
  xValue = analogRead(A0) / 4; // for sim in wokwi
  setPoint = xValue;
  static int lastSetPoint = 0;
  if (setPoint != lastSetPoint) {
    if (setPoint < 116) {
      mappedsetPoint = map(xValue, 116, 0, 0, -maxMotorRPM); //Reverse mapping
    } else if (setPoint > 121) {
      mappedsetPoint = map(xValue, 120, 250, 0, maxMotorRPM);  // Forward mapping
    } else {
      mappedsetPoint = 0;
    }
  }
  output = computePID();
  digitalWrite(dirPin, output < 0 ?  LOW : HIGH);
  analogWrite(pwmPin, abs(output));               // Write new PID output to the pin
  simMotor();
  report();
}

void encoder() {
  counter++;
}

void computerpm() {
  counter = (digitalRead(dirPin)? 1:-1)* abs(simSpeed)  * 96 / 240 ;// override with simMotor speed
  RPM1 = (counter / 96.0) * 240;  // Calculate RPM1 from the counter value
  counter = 0;
  previousTime = millis();  // Update previousTime here
  return RPM1;
}

double computePID() {
  static double out = 0;
  currentTime = millis();                              // Get current time
  elapsedTime = (double)(currentTime - previousTime);  // Compute time elapsed from previous computation
  if (elapsedTime < 100) return out;
  previousTime = currentTime;                                // Update previousTime
  error = mappedsetPoint - RPM1;                             // Determine error
  cumError += error * elapsedTime / 1000.0;                  // Compute integral
  rateError = (error - lastError) / elapsedTime * 1000.0;    // Compute derivative
  out = kp * error + ki * cumError + kd * rateError;  // PID output
  // Undo integral if out of limits
  if (out > 255) {
    cumError -= error * elapsedTime / 1000.0;
    out = 255;
  }
  if (out < -255) {
    cumError -= error * elapsedTime / 1000.0;
    out = -255;
  }
  lastError = error;  // Remember the current error
  return out;         // Return PID output (PWM)
}

void report(void) {
  static unsigned long last = 0;
  const unsigned long interval = 500;
  unsigned long now = millis();
  if (now - last < interval) return;
  last = now;

  //   Serial.print(now);
  //   Serial.print("ms");

  Serial.print("xValue:");
  Serial.print(xValue);
  Serial.print(", setPoint: ");
  Serial.print(setPoint);
  Serial.print(", mappedsetPoint:");
  Serial.print(mappedsetPoint);
  Serial.print(", output:");
  Serial.print(output);
  Serial.print(", dir:");
  Serial.print(output>0);
  Serial.print(", pwm:");
  Serial.print(abs(output));
  Serial.print(", simSpeed:");
  Serial.print(simSpeed);
  Serial.print(", RPM1:");
  Serial.print(RPM1);
  Serial.println();
}

void simMotor(void){
  // simulate a motor based on PWM and DIR
  // periodically smooth speed towards the full-speed RPM
   static float speed = 0.0;
   static unsigned long last = 0;
   unsigned long now = millis();
   if (now -last < 100) return;
   last = now;
  float volts = 1.0 * abs(output)/255 * (digitalRead(dirPin)==HIGH? 1 : -1) ;
  const float VToRPM = maxMotorRPM/1.0;
  speed += 0.1 * (volts * VToRPM - speed);
   
  simSpeed = speed;
}