#include <Arduino.h>
#include <WiFi.h>
#include "config.h"
#include "motor.h"
#include "webserver.h"

// Coordinated movement state
enum CoordinatedState {
    COORD_IDLE = 0,
    COORD_MOVING_REV = 1,   // Homing - moving to reverse end
    COORD_MOVING_FWD = 2    // Moving to forward end
};

// Global coordinated movement state
static CoordinatedState coordState = COORD_IDLE;
static bool motor1Done = false;
static bool motor2Done = false;
static int coordSpeed = 50;  // Default coordinated movement speed

// Get current coordinated movement state
int getCoordinatedState() {
    return (int)coordState;
}

// Check if coordinated movement is complete
bool isCoordinatedMovementDone() {
    return motor1Done && motor2Done;
}

// Called when motor 1 stall is detected
void onMotor1Stall() {
    Serial.println("Motor1 stall callback triggered - stopping motor!");
    motor1.stop();
    
    // Mark motor1 as done if in coordinated movement
    if (coordState != COORD_IDLE) {
        motor1Done = true;
        Serial.println("Motor1 reached end position (stall)");
        
        if (isCoordinatedMovementDone()) {
            Serial.println("Coordinated movement complete - both motors at end position");
        }
    }
}

// Called when motor 2 stall is detected
void onMotor2Stall() {
    Serial.println("Motor2 stall callback triggered - stopping motor!");
    motor2.stop();
    
    // Mark motor2 as done if in coordinated movement
    if (coordState != COORD_IDLE) {
        motor2Done = true;
        Serial.println("Motor2 reached end position (stall)");
        
        if (isCoordinatedMovementDone()) {
            Serial.println("Coordinated movement complete - both motors at end position");
        }
    }
}

// Start coordinated movement in specified direction
// dir: -1 = reverse (homing), 1 = forward
void startCoordinatedMovement(int dir, int speed) {
    coordSpeed = speed;
    motor1Done = false;
    motor2Done = false;
    
    if (dir < 0) {
        coordState = COORD_MOVING_REV;
        Serial.printf("Starting coordinated REVERSE movement at %d%% speed\n", speed);
    } else {
        coordState = COORD_MOVING_FWD;
        Serial.printf("Starting coordinated FORWARD movement at %d%% speed\n", speed);
    }
    
    // Start both motors
    motor1.setSpeed(speed);
    motor1.setDirection(dir);
    
    motor2.setSpeed(speed);
    motor2.setDirection(dir);
}

// Toggle coordinated movement (for button press)
// Returns: new state (0=idle, 1=moving_rev, 2=moving_fwd)
int toggleCoordinatedMovement(int speed) {
    if (coordState == COORD_IDLE) {
        // First press: start moving reverse (homing)
        startCoordinatedMovement(-1, speed);
        return COORD_MOVING_REV;
    } else if (coordState == COORD_MOVING_REV) {
        // If currently moving rev or completed rev, switch to forward
        // Stop any still-moving motors first
        if (!motor1Done) motor1.stop();
        if (!motor2Done) motor2.stop();
        startCoordinatedMovement(1, speed);
        return COORD_MOVING_FWD;
    } else {
        // Currently moving fwd or completed fwd, switch to reverse
        // Stop any still-moving motors first
        if (!motor1Done) motor1.stop();
        if (!motor2Done) motor2.stop();
        startCoordinatedMovement(-1, speed);
        return COORD_MOVING_REV;
    }
}

// Stop coordinated movement
void stopCoordinatedMovement() {
    coordState = COORD_IDLE;
    motor1Done = false;
    motor2Done = false;
    motor1.stop();
    motor2.stop();
    Serial.println("Coordinated movement stopped");
}

void setupWiFi() {
    Serial.println("Connecting to WiFi...");
    
    // Configure static IP
    if (!WiFi.config(STATIC_IP, GATEWAY, SUBNET, DNS)) {
        Serial.println("Static IP configuration failed!");
    }
    
    WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
    
    int attempts = 0;
    while (WiFi.status() != WL_CONNECTED && attempts < 30) {
        delay(500);
        Serial.print(".");
        attempts++;
    }
    
    if (WiFi.status() == WL_CONNECTED) {
        Serial.println("\nWiFi connected!");
        Serial.print("IP Address: ");
        Serial.println(WiFi.localIP());
    } else {
        Serial.println("\nWiFi connection failed!");
        Serial.println("Restarting in 5 seconds...");
        delay(5000);
        ESP.restart();
    }
}

void setup() {
    Serial.begin(115200);
    delay(1000);
    
    Serial.println("\n=============================");
    Serial.println("  Dual BTS7960 Motor Controller");
    Serial.println("  Differential Drive Robot");
    Serial.println("=============================\n");
    
    // Initialize motor controllers
    motor1.begin();
    motor1.setStallCallback(onMotor1Stall);
    
    motor2.begin();
    motor2.setStallCallback(onMotor2Stall);
    
    // Connect to WiFi
    setupWiFi();
    
    // Start web server
    setupWebServer();
    
    Serial.println("\nReady! Access the control panel at:");
    Serial.print("http://");
    Serial.println(WiFi.localIP());
}

void loop() {
    handleWebServer();
    motor1.update();  // Update current sensing and logging for motor 1
    motor2.update();  // Update current sensing and logging for motor 2
    delay(1);
}