#include <Wire.h>
#include "Handbrake.h"  // Include the Handbrake class header

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

// Pin definitions
const int engageButtonPin = A2;
const int disengageButtonPin = A3;
const int torquePotPin = A0;
const int speedPotPin = A1;
const int motorPin1 = 4;  // Motor control pin 1
const int motorPin2 = 5;  // Motor control pin 2

Handbrake handbrake(motorPin1, motorPin2);

void setup() {
    Serial.begin(9600);
    pinMode(engageButtonPin, INPUT_PULLUP);
    pinMode(disengageButtonPin, INPUT_PULLUP);   
}

void loop() {
    // Read button states
    bool engageButtonState = digitalRead(engageButtonPin) == LOW;
    bool disengageButtonState = digitalRead(disengageButtonPin) == LOW;

    // Read torque and speed from potentiometers
    float torque = analogRead(torquePotPin) * (100.0 / 1023.0);
    float speed = analogRead(speedPotPin) * (100.0 / 1023.0);

    // Update the handbrake state
    handbrake.update(engageButtonState, disengageButtonState, torque, speed);

    // Debug output
    Serial.print("Torque: "); Serial.print(torque);
    Serial.print("%, Speed: "); Serial.print(speed);
    Serial.print(" Kph, Handbrake Engaged: "); Serial.print(handbrake.isEngaged());
    Serial.print(", Manual Mode: "); Serial.println(handbrake.isManualMode());

    delay(100); // Short delay for readability
}