/**
 * Claude's Eyes - Main Program
 *
 * Autonomous exploration robot controlled by Claude AI
 *
 * Hardware:
 * - Waveshare ESP32-S3-Touch-LCD-2
 * - OV5640 Camera (120° wide-angle)
 * - Freenove 4WD Car Kit
 *
 * Created: December 2025
 * By: Stefan (HackerSoft) & Claude (Anthropic)
 */

#include <Arduino.h>
#include <WiFi.h>
#include "config.h"
#include "camera.h"
#include "motor_control.h"
#include "servo_control.h"
#include "ultrasonic.h"
#include "imu.h"
#include "display.h"
#include "webserver.h"

// State machine
enum RobotState {
    STATE_INITIALIZING,
    STATE_CONNECTING,
    STATE_READY,
    STATE_EXPLORING,
    STATE_OBSTACLE,
    STATE_ERROR
};

RobotState currentState = STATE_INITIALIZING;
unsigned long lastCommandTime = 0;
unsigned long lastStatusUpdate = 0;

// Forward declarations
void setupWiFi();
void handleSafety();
void handleTouchInput();
void updateStatusDisplay();

void setup() {
    // Initialize serial
    Serial.begin(DEBUG_BAUD_RATE);
    while (!Serial && millis() < 3000) {
        delay(10);
    }
    Serial.println("\n\n");
    Serial.println("╔═══════════════════════════════════════╗");
    Serial.println("║        Claude's Eyes v1.0             ║");
    Serial.println("║   Autonomous Exploration Robot        ║");
    Serial.println("╚═══════════════════════════════════════╝");
    Serial.println();

    // Initialize display first (for visual feedback)
    Serial.println("[Main] Initializing display...");
    if (Display.begin()) {
        Display.showMessage("Starting...", TFT_CYAN);
    }
    delay(500);

    // Initialize camera
    Serial.println("[Main] Initializing camera...");
    if (!Camera.begin()) {
        Serial.println("[Main] WARNING: Camera init failed!");
        Display.showMessage("Camera Error!", TFT_RED);
        delay(2000);
    } else {
        Serial.println("[Main] Camera ready");
    }

    // Initialize motors
    Serial.println("[Main] Initializing motors...");
    if (!Motors.begin()) {
        Serial.println("[Main] WARNING: Motors init failed!");
    } else {
        Serial.println("[Main] Motors ready");
    }

    // Initialize servos
    Serial.println("[Main] Initializing servos...");
    if (!Servos.begin()) {
        Serial.println("[Main] WARNING: Servos init failed!");
    } else {
        Serial.println("[Main] Servos ready");
        // Center the camera
        Servos.center();
    }

    // Initialize ultrasonic sensor
    Serial.println("[Main] Initializing ultrasonic sensor...");
    if (!Ultrasonic.begin()) {
        Serial.println("[Main] WARNING: Ultrasonic init failed!");
    } else {
        Serial.println("[Main] Ultrasonic ready");
    }

    // Initialize IMU
    Serial.println("[Main] Initializing IMU...");
    if (!IMU.begin()) {
        Serial.println("[Main] WARNING: IMU init failed!");
    } else {
        Serial.println("[Main] IMU ready");
    }

    // Connect to WiFi
    currentState = STATE_CONNECTING;
    Display.showMessage("Connecting WiFi...", TFT_YELLOW);
    setupWiFi();

    // Start webserver
    Serial.println("[Main] Starting webserver...");
    if (!WebServer.begin()) {
        Serial.println("[Main] ERROR: WebServer failed!");
        currentState = STATE_ERROR;
        Display.showMessage("Server Error!", TFT_RED);
        return;
    }

    // All systems go!
    currentState = STATE_READY;
    Display.setMode(MODE_STATUS);
    Display.showEmoji(EMOJI_HAPPY);
    delay(1000);
    Display.setMode(MODE_STATUS);

    Serial.println();
    Serial.println("╔═══════════════════════════════════════╗");
    Serial.println("║          SYSTEM READY                 ║");
    Serial.printf( "║   IP: %-30s ║\n", WiFi.localIP().toString().c_str());
    Serial.println("║   Waiting for Claude...               ║");
    Serial.println("╚═══════════════════════════════════════╝");
    Serial.println();

    lastCommandTime = millis();
    lastStatusUpdate = millis();
}

void loop() {
    unsigned long now = millis();

    // Update sensors
    Ultrasonic.update();
    IMU.update();
    Servos.update();
    Motors.update();

    // Handle safety checks
    handleSafety();

    // Handle touch input
    handleTouchInput();

    // Update display periodically
    if (now - lastStatusUpdate >= DISPLAY_UPDATE_INTERVAL) {
        lastStatusUpdate = now;
        updateStatusDisplay();
    }

    // Command timeout safety
    if (Motors.isMoving() && (now - lastCommandTime > COMMAND_TIMEOUT_MS)) {
        Serial.println("[Main] Command timeout - stopping");
        Motors.stop();
    }

    // Small delay to prevent watchdog issues
    delay(1);
}

void setupWiFi() {
    Serial.printf("[WiFi] Connecting to %s", WIFI_SSID);

    WiFi.mode(WIFI_STA);
    WiFi.begin(WIFI_SSID, WIFI_PASSWORD);

    int attempts = 0;
    while (WiFi.status() != WL_CONNECTED && attempts < 30) {
        delay(500);
        Serial.print(".");
        attempts++;

        // Update display
        if (attempts % 2 == 0) {
            char msg[32];
            snprintf(msg, sizeof(msg), "WiFi... %d", attempts);
            Display.showMessage(msg, TFT_YELLOW);
        }
    }

    if (WiFi.status() == WL_CONNECTED) {
        Serial.println(" Connected!");
        Serial.printf("[WiFi] IP Address: %s\n", WiFi.localIP().toString().c_str());
        Serial.printf("[WiFi] Signal strength: %d dBm\n", WiFi.RSSI());

        char msg[32];
        snprintf(msg, sizeof(msg), "IP: %s", WiFi.localIP().toString().c_str());
        Display.showMessage(msg, TFT_GREEN);
        delay(2000);
    } else {
        Serial.println(" FAILED!");
        Serial.println("[WiFi] Could not connect to WiFi");

        // Start AP mode as fallback
        Serial.println("[WiFi] Starting AP mode...");
        WiFi.mode(WIFI_AP);
        WiFi.softAP("ClaudesEyes", "claude2025");
        Serial.printf("[WiFi] AP IP: %s\n", WiFi.softAPIP().toString().c_str());

        Display.showMessage("AP: ClaudesEyes", TFT_ORANGE);
        delay(2000);
    }
}

void handleSafety() {
    // Check for obstacles
    if (Ultrasonic.isDanger() && Motors.isMoving()) {
        MotorDirection dir = Motors.getCurrentDirection();
        if (dir == DIR_FORWARD) {
            Serial.println("[Safety] Obstacle too close - stopping!");
            Motors.stop();
            Display.showEmoji(EMOJI_SURPRISED);

            // Set Claude text about obstacle
            WebServer.setClaudeText("Hindernis erkannt! Ich halte an.");
        }
    }

    // Check if robot is tilted
    if (IMU.isTilted() && Motors.isMoving()) {
        Serial.println("[Safety] Robot tilted - stopping!");
        Motors.stop();
        Display.showEmoji(EMOJI_CONFUSED);
    }

    // Check if upside down
    if (IMU.isUpsideDown()) {
        Serial.println("[Safety] Robot upside down!");
        Motors.stop();
        Motors.setEnabled(false);  // Disable motors completely
        Display.showMessage("HILFE!", TFT_RED);
    }
}

void handleTouchInput() {
    TouchButton btn = Display.checkTouch();

    if (btn == BTN_NONE) return;

    lastCommandTime = millis();

    switch (btn) {
        case BTN_STOP:
            Serial.println("[Touch] STOP pressed");
            Motors.stop();
            Display.showEmoji(EMOJI_SLEEPY);
            break;

        case BTN_FORWARD:
            Serial.println("[Touch] Forward pressed");
            Motors.move(DIR_FORWARD, 50, 1000);
            break;

        case BTN_BACKWARD:
            Serial.println("[Touch] Backward pressed");
            Motors.move(DIR_BACKWARD, 50, 1000);
            break;

        case BTN_LEFT:
            Serial.println("[Touch] Left pressed");
            Motors.move(DIR_LEFT, 50, 500);
            break;

        case BTN_RIGHT:
            Serial.println("[Touch] Right pressed");
            Motors.move(DIR_RIGHT, 50, 500);
            break;

        case BTN_HOME:
            Serial.println("[Touch] Home pressed");
            // TODO: Implement return to base
            Display.showMessage("Coming home!", TFT_CYAN);
            break;

        default:
            break;
    }
}

void updateStatusDisplay() {
    if (Display.isInitialized()) {
        Display.updateStatus(
            Motors.getDirectionString(),
            Ultrasonic.getLastDistance(),
            WebServer.getBatteryPercent(),
            WiFi.RSSI()
        );
    }
}