#include <WiFi.h>
#include "esp_camera.h"
#include "esp_http_server.h"
#include <VL53L0X.h>  // Pololu library

#include <HTTPClient.h>
#include <string>


#// Uncomment the following line to enable POSTing frames to the Flask server
//#define SEND_TO_SERVER

// Set this to your Flask server (e.g. "http://192.168.1.100:5000/_data") when SEND_TO_SERVER is enabled
// const char* SERVER_URL = "http://<SERVER_IP>:5000/_data";

#define CAMERA_MODEL_XIAO_ESP32S3
#include "camera_pins.h"



//=== DISTANCE CONFIG ===
#define LONG_RANGE
#define HIGH_ACCURACY
#define SAFETY_DIST 150 //mm
VL53L0X sensor;

//=== DISTANCE CONFIG ===


// NOTE PLACEHOLDERS, don't know what to send 
typedef enum {
  LEFT = 0b10000,
  RIGHT = 0b10000, 
  FORWARD = 0b10101,
  BACKWARD = ~FORWARD,
  START = 0b1111111,
  STOP = 0b0000000,
  ROTATE_L_INPLACE = 0x7d,
  ROTATE_R_INPLACE = 0x7d,


} DriveCmd;

typedef enum {
    IDLE, // robot has paused.
    FOLLOWING, // sees the target so is following it 
    ROAMING, // cannot see a target so just roaming around 
    ERROR
} SystemState;

// Define the state struct

class MobileBot
{
  public:
    // Data member
    DriveCmd currCmd; 
    SystemState state; 
    bool viewingTarget;

    MobileBot(DriveCmd cmd, SystemState s){
      currCmd = cmd; 
      state = s;
      viewingTarget = false; 
    }

    void stateCallback(bool, uint16_t);
    void driveCallback();

    
};
// void MobileBot::stateCallback(bool hasTarget, uint16_t laser_dist){
//   viewingTarget = hasTarget; 
//   if (~viewingTarget) {
//     if (laser_dist <= SAFETY_DIST){
//       currCmd = IDLE;
//     }else{
//       currCmd = ROAMING;
//     }
    

//   }else { // target in view 
//     if (laser <= SAFETY_DIST){
//       currCmd = IDLE;
//     }
//     currCmd = FOLLOWING;
//   }
  

// }; // add additional stuff later 

struct RobotState {
    SystemState driveState;
    DriveCmd cmd; 
};


// ==== WIFI ====
const char* ssid     = "EDS_Shop_24";
const char* password = "eds12345";


// Forward declarations
static esp_err_t stream_handler(httpd_req_t *req);
void startCameraServer();
void scanI2C();

void setup() {
  delay(1500);
  Serial.begin(115200);
  while (!Serial) { delay(10); }
  Wire.setClock(400000); // use 400 kHz I2C

  Serial.println("\nBooting the VL53L0X sensor");

  // === SENSOR CONFIG ==== 
  scanI2C();
  sensor.setTimeout(5000);
  sensor.setBus(&Wire); // use Wire1 to use I2C-1
  uint16_t address = sensor.readReg16Bit(sensor.IDENTIFICATION_MODEL_ID);
  char addr_buffer[64];
  sprintf(addr_buffer, "Sensor identitfaction model: 0x%x\n", address);
  Serial.println(addr_buffer);
  int status = sensor.init();
  char buffer[64];
  sprintf(buffer, "Sensor init status: %d\n", status);
  Serial.println(buffer);
  if (!sensor.init()) {
    Serial.println("Failed to detect VL53L0X!");
    while (1);
  }

  #if defined LONG_RANGE
    // lower the return signal rate limit (default is 0.25 MCPS)
    sensor.setSignalRateLimit(0.1);
    // increase laser pulse periods (defaults are 14 and 10 PCLKs)
    sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
    sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
  #endif

  #if defined HIGH_SPEED
    // reduce timing budget to 20 ms (default is about 33 ms)
    sensor.setMeasurementTimingBudget(20000);
  #elif defined HIGH_ACCURACY
    // increase timing budget to 200 ms
    sensor.setMeasurementTimingBudget(200000);
  #endif
  Serial.println("VL53L0X initialized!");
  // ====== END SENSOR CONFIG =====

  Serial.println("\nBooting XIAO ESP32S3 Camera...");

  // ==== CAMERA CONFIG ====
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer   = LEDC_TIMER_0;

  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;

  config.pin_xclk  = XCLK_GPIO_NUM;
  config.pin_pclk  = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href  = HREF_GPIO_NUM;

  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;

  config.pin_pwdn  = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;

  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;

  if (psramFound()) {
    config.frame_size = FRAMESIZE_QVGA;
    config.jpeg_quality = 12;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_QQVGA;
    config.fb_count = 1;
  }

  Serial.println("Initializing camera...");
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed 0x%x\n", err);
    while (true) delay(100);
  }
  Serial.println("Camera OK!");

  // ==== WIFI ====
  Serial.println("Connecting to WiFi...");
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.print("\nWiFi connected. IP: ");
  Serial.println(WiFi.localIP());

  startCameraServer();
}

RobotState drive(uint16_t laser_dist, RobotState prevState){
  RobotState driveCmd;
  if (laser_dist > SAFETY_DIST){
    return {ROAMING, FORWARD};
  } else if (laser_dist <= SAFETY_DIST) {
    if (prevState.cmd == FORWARD){
      return {ROAMING, ROTATE_L_INPLACE};
    } else if (prevState.cmd == ROTATE_L_INPLACE) {
      return {ROAMING, ROTATE_R_INPLACE};
    }
    return {IDLE, STOP};

  }
}


void loop() {
  uint16_t distance = sensor.readRangeSingleMillimeters();
  Serial.print(distance);
  Serial.println(" mm");
  if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  Serial.println();


}

// ==== STREAM HANDLER ====
static httpd_handle_t camera_httpd = NULL;

static esp_err_t stream_handler(httpd_req_t *req) {
  camera_fb_t *fb = NULL;
  esp_err_t res = ESP_OK;
  char buf[64];

  httpd_resp_set_type(req, "multipart/x-mixed-replace; boundary=frame");

  while (true) {
    fb = esp_camera_fb_get();
    if (!fb) {
      Serial.println("Camera capture failed");
      return ESP_FAIL;
    }

    size_t hlen = snprintf(buf, 64,
      "--frame\r\nContent-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n",
      fb->len);

    res = httpd_resp_send_chunk(req, buf, hlen);
    if (res == ESP_OK)
      res = httpd_resp_send_chunk(req, (const char*)fb->buf, fb->len);

    // --- Optional: forward captured frame to remote Flask server ---
#if defined(SEND_TO_SERVER)
    {
      static const char *server_url = "http://<SERVER_IP>:5000/_data"; // set to your Flask server
      HTTPClient http;
      http.begin(server_url);
      http.addHeader("Content-Type", "image/jpeg");
      int httpCode = http.POST((uint8_t *)fb->buf, fb->len);
      if (httpCode > 0) {
        Serial.printf("Posted to server, code=%d\n", httpCode);
      } else {
        Serial.printf("Post failed, error=%d\n", httpCode);
      }
      http.end();
    }
#endif

    esp_camera_fb_return(fb);

    if (res != ESP_OK)
      break;

    res = httpd_resp_send_chunk(req, "\r\n", 2);
    if (res != ESP_OK)
      break;
  }

  return res;
}

// ==== START SERVER ====
void startCameraServer() {
  httpd_config_t config = HTTPD_DEFAULT_CONFIG();

  httpd_uri_t uri = {
    .uri      = "/",
    .method   = HTTP_GET,
    .handler  = stream_handler,
    .user_ctx = NULL
  };

  httpd_start(&camera_httpd, &config);
  httpd_register_uri_handler(camera_httpd, &uri);

  Serial.println("\n===== CAMERA STREAM READY =====");
  Serial.print("Open in browser: http://");
  Serial.print(WiFi.localIP());
  Serial.println("/");
}