#include <Arduino.h>
#include <math.h>

// ========================================
// P2: ESP32 FREERTOS PID VELOCITY CONTROL
// ========================================

// Pin motor
#define MOTOR_P       25
#define MOTOR_N       26

// Pin encoder
#define ENCODER_A     34
#define ENCODER_B     35

// Potensiometer setpoint
#define SETPOINT_PIN  33

// Konfigurasi encoder (nilai awal)
#define PULSES_PER_REV 100.0f

// PWM
#define PWM_FREQ      5000
#define PWM_RES       8

// Sampling time
#define SAMPLE_MS     100

// PID BARU
float Kp = 1.2f;
float Ki = 0.8f;
float Kd = 0.0f;

// Data bersama
volatile long encoderCount = 0;

portMUX_TYPE encoderMux = portMUX_INITIALIZER_UNLOCKED;
portMUX_TYPE dataMux = portMUX_INITIALIZER_UNLOCKED;

int adcValue = 0;
int pwmValue = 0;
float targetRPM = 0.0f;
float actualRPM = 0.0f;
float errorRPM = 0.0f;

float integral = 0.0f;
float previousError = 0.0f;

// ========================================
// ENCODER INTERRUPT
// ========================================

void IRAM_ATTR encoderISR() {
  portENTER_CRITICAL_ISR(&encoderMux);
  encoderCount++;
  portEXIT_CRITICAL_ISR(&encoderMux);
}

// ========================================
// TASK 1: SETPOINT
// ========================================

void taskSetpoint(void *pv) {
  for (;;) {
    int raw = analogRead(SETPOINT_PIN);
    float rpm = raw * 300.0f / 4095.0f;

    portENTER_CRITICAL(&dataMux);
    adcValue = raw;
    targetRPM = rpm;
    portEXIT_CRITICAL(&dataMux);

    vTaskDelay(pdMS_TO_TICKS(50));
  }
}

// ========================================
// TASK 2: RPM AKTUAL
// ========================================

void taskRPM(void *pv) {
  long previousCount = 0;

  for (;;) {
    vTaskDelay(pdMS_TO_TICKS(SAMPLE_MS));

    long currentCount;

    portENTER_CRITICAL(&encoderMux);
    currentCount = encoderCount;
    portEXIT_CRITICAL(&encoderMux);

    long delta = currentCount - previousCount;
    previousCount = currentCount;

    float rpm =
      fabsf((float)delta) * 60000.0f
      / (PULSES_PER_REV * SAMPLE_MS);

    portENTER_CRITICAL(&dataMux);
    actualRPM = rpm;
    portEXIT_CRITICAL(&dataMux);
  }
}

// ========================================
// TASK 3: PID CONTROL
// ========================================

void taskPID(void *pv) {
  const float dt = SAMPLE_MS / 1000.0f;

  for (;;) {
    vTaskDelay(pdMS_TO_TICKS(SAMPLE_MS));

    float target, actual;

    portENTER_CRITICAL(&dataMux);
    target = targetRPM;
    actual = actualRPM;
    portEXIT_CRITICAL(&dataMux);

    float error = target - actual;
    int outputPWM = 0;

    if (target >= 1.0f) {
      integral += error * dt;
      integral = constrain(integral, -200.0f, 200.0f);

      float derivative = (error - previousError) / dt;

      float output =
        Kp * error +
        Ki * integral +
        Kd * derivative;

      outputPWM = (int)constrain(output, 0.0f, 255.0f);
    } else {
      integral = 0.0f;
      previousError = 0.0f;
      outputPWM = 0;
    }

    previousError = error;

    // Kendali motor satu arah
    ledcWrite(MOTOR_N, 0);
    ledcWrite(MOTOR_P, outputPWM);

    portENTER_CRITICAL(&dataMux);
    errorRPM = error;
    pwmValue = outputPWM;
    portEXIT_CRITICAL(&dataMux);
  }
}

// ========================================
// TASK 4: MONITOR
// ========================================

void taskMonitor(void *pv) {
  Serial.println();
  Serial.println("P2 - PID VELOCITY CONTROL");
  Serial.println("Kp=1.2 Ki=0.8 Kd=0.0");
  Serial.println("ADC | Target RPM | Actual RPM | PWM | Count");

  for (;;) {
    int adc, pwm;
    float target, actual;
    long count;

    portENTER_CRITICAL(&dataMux);
    adc = adcValue;
    target = targetRPM;
    actual = actualRPM;
    pwm = pwmValue;
    portEXIT_CRITICAL(&dataMux);

    portENTER_CRITICAL(&encoderMux);
    count = encoderCount;
    portEXIT_CRITICAL(&encoderMux);

    Serial.printf(
      "%4d | %10.1f | %10.1f | %3d | %ld\n",
      adc, target, actual, pwm, count
    );

    vTaskDelay(pdMS_TO_TICKS(250));
  }
}

// ========================================
// SETUP
// ========================================

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

  pinMode(SETPOINT_PIN, INPUT);
  pinMode(ENCODER_A, INPUT);
  pinMode(ENCODER_B, INPUT);

  bool pwmP = ledcAttach(MOTOR_P, PWM_FREQ, PWM_RES);
  bool pwmN = ledcAttach(MOTOR_N, PWM_FREQ, PWM_RES);

  ledcWrite(MOTOR_P, 0);
  ledcWrite(MOTOR_N, 0);

  attachInterrupt(
    digitalPinToInterrupt(ENCODER_A),
    encoderISR,
    CHANGE
  );

  Serial.printf("PWM attach P=%d N=%d\n", pwmP, pwmN);
  Serial.println("Memulai task FreeRTOS...");

  xTaskCreatePinnedToCore(
    taskSetpoint, "Setpoint", 2048, NULL, 1, NULL, 1
  );

  xTaskCreatePinnedToCore(
    taskRPM, "RPM", 2048, NULL, 2, NULL, 0
  );

  xTaskCreatePinnedToCore(
    taskPID, "PID", 4096, NULL, 2, NULL, 1
  );

  xTaskCreatePinnedToCore(
    taskMonitor, "Monitor", 3072, NULL, 1, NULL, 0
  );
}

void loop() {
  vTaskDelay(pdMS_TO_TICKS(1000));
}