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

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();
    motor2.begin();
    
    // 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);
}