// Created 09/17/24 - DW

#include <Encoder.h>

#define MOTOR_IN1 8        // Actual PIN#s TBD later
#define MOTOR_IN2 9
#define EXTEND_BUTTON 11
#define RETRACT_BUTTON 13
#define ENCODER_CH_A 2
#define ENCODER_CH_B 4

#define CPR 230.7           // Counts per revolution of motor encoder
#define REVOLUTIONS 5       // Number of revolutions for max blade extension
#define DEADZONE 10         // Stops PID loop if error is within this many encoder ticks
#define PWM_MAX 255         // PWM range
#define P 1.0               
#define I 0.01              
#define D 0.1               
#define DEBOUNCE_DELAY 50   // Debounce delay in ms
#define PID_LOOP_TIME 10    // PID loop time in ms

Encoder encoder(ENCODER_CH_A, ENCODER_CH_B);
int extendState, lastExtendState = HIGH, retractState, lastRetractState = HIGH, encoderCount = 0, pwmValue = 0;
float targetCounts = 0, error = 0, previousError = 0, integral = 0, derivative = 0, output = 0;
unsigned long lastButtonPress = 0, currentTime = 0, lastTime = 0, elapsedTime = 0;

void setup() {
    Serial.begin(9600);
    pinMode(MOTOR_IN1, OUTPUT);
    pinMode(MOTOR_IN2, OUTPUT);
    pinMode(EXTEND_BUTTON, INPUT_PULLUP);
    pinMode(RETRACT_BUTTON, INPUT_PULLUP);
}

void loop() {
    // Button Handling
    extendState = digitalRead(EXTEND_BUTTON);
    retractState = digitalRead(RETRACT_BUTTON);

    if (extendState != lastExtendState || retractState != lastRetractState) {
        lastButtonPress = millis();
    }
    if ((millis() - lastButtonPress) > DEBOUNCE_DELAY) {
        if (extendState == LOW && lastExtendState == HIGH) {
            targetCounts = REVOLUTIONS * CPR;
            Serial.println("Extending");
        }
        else if (retractState == LOW && lastRetractState == HIGH) {
            targetCounts = 0;
            Serial.println("Retracting");
        }
        else {
            Serial.println("Nothing");
        }
    }

    lastExtendState = extendState;
    lastRetractState = retractState;

    // PID Loop
    currentTime = millis();
    elapsedTime = currentTime - lastTime;

    if (elapsedTime >= PID_LOOP_TIME) {
        encoderCount = encoder.read();
        error = targetCounts - encoderCount;

        if (abs(error) < DEADZONE) {
            analogWrite(MOTOR_IN1, 0);
            analogWrite(MOTOR_IN2, 0);
        }
        else {
            integral += error * (elapsedTime / 1000.0);
            derivative = (error - previousError) / (elapsedTime / 1000.0);

            output = P * error + I * integral + D * derivative;

            pwmValue = constrain(abs(output), 0, PWM_MAX);

            if (output > 0) {
                analogWrite(MOTOR_IN1, pwmValue);
                analogWrite(MOTOR_IN2, 0);
            }
            else {
                analogWrite(MOTOR_IN1, 0);
                analogWrite(MOTOR_IN2, pwmValue);
            }
        }

        previousError = error;
        lastTime = currentTime;

        Serial.print("Encoder Count: ");
        Serial.println(encoderCount);
        Serial.print("Error: ");
        Serial.println(error, 3);
        Serial.print("Output: ");
        Serial.println(output, 3);
        Serial.println();
    }
}