#include <TimerOne.h>

const int encoderPinA = 2;
const int pwmPin = 4;   // Motor PWM
const int dirPin = 53;  // Motor DIR

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

unsigned long currentTime, previousTime;
double elapsedTime;

// PID declaration
double kp = 0.06;   // Proportional gain
double ki = 0.1;    // Integral gain
double kd = 0.082;  // Derivative gain

double error;
double lastError = 0; // Initialize lastError to 0
double input, output;
int setPoint;
double cumError = 0;  // Initialize cumError to 0
double rateError;
uint32_t loopcounter = 0;
void setup() {
  Serial.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() {
  setPoint = 80;
  currentRPM = RPM;
  //Serial.println(currentRPM); // Print RPM value

  //cumError = 0; // Reset cumError at the beginning of each control loop
  output = computePID();
  analogWrite(pwmPin, output); // Write new PID output to the pin
  simulate();
  report();

  ++loopcounter;
}

void encoder() {
  counter++;
}

double computerpm() {
  RPM = (counter / 96) * 240;  // Calculate RPM from the counter value
  counter = 0;
  previousTime = millis();  // Update previousTime here

  return RPM;
}

double computePID() {
  currentTime = millis();  // Get current time
  elapsedTime = (double)(currentTime - previousTime);  // Compute time elapsed from previous computation
  if (elapsedTime < 100) return;
  previousTime = currentTime;  // Update previousTime

  error = setPoint - RPM;  // Determine error
  cumError += error * elapsedTime/1000.0;  // Compute integral
  rateError = (error - lastError) / elapsedTime *1000.0;  // Compute derivative

  double 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 < 0) {cumError -= error * elapsedTime/1000.0; out = 0;}

  lastError = error;  // Remember the current error
  return out;  // Return PID output (PWM)
}

// extra bits for reporting and simulating in WokWi with a slide-pot attached to A0

void simulate(void) {
  static uint32_t last = 0;
  const uint32_t interval = 5;
  uint32_t now = millis();
  if (now - last < interval) return;
  last = now;
  //encoder();
  //if(analogRead(A0) >= random(1023)) encoder();
  counter = analogRead(A0) * 100L/1023;
}


void report(void) {
  static uint32_t last = 0;
  const uint32_t interval = 250;
  uint32_t now = millis();
  if (now - last < interval) return;
  last = now;
  Serial.print("RPM:");
  Serial.print(currentRPM); // Print RPM value
  Serial.print(" Output:");
  Serial.print(output);
  Serial.print(" cumError:");
  Serial.print(cumError);
  Serial.print("rpm*s kLoop/s:");
  Serial.print(loopcounter*1.0/interval);
  Serial.print("");
  loopcounter=0;
  Serial.println();
  //Serial.println(currentRPM); // Print RPM value
}