/*
  ESP32CAM Robot Car Adapted for L298N Motor Driver
  This code sets up an ESP32-CAM as an access point, starts the camera server,
  and provides basic motor control using the L298N driver.
  
  Based on the DroneBot Workshop 2021 project.
*/

#include "esp_wifi.h"
#include "esp_camera.h"
#include <WiFi.h>
#include "soc/soc.h"
#include "soc/rtc_cntl_reg.h"

// Access Point Credentials
const char* ssid1 = "ESP32-CAM Robot";
const char* password1 = "1234567890";

// Motor Pins for L298N
#define IN1 12  // Left motor forward
#define IN2 13  // Left motor reverse
#define IN3 14  // Right motor forward
#define IN4 15  // Right motor reverse

// LED pin (for status indication)
#define LED_PIN 4

// Global Variables for Motor Control
volatile unsigned int motor_speed = 50;  // Adjust speed value as needed (0-255)
volatile unsigned long previous_time = 0;
volatile unsigned long move_interval = 5000; // Example time in milliseconds
volatile uint8_t robo = 1;  // Trigger motor movement sequence

// Camera Module Pin Configuration
#define CAMERA_MODEL_AI_THINKER
#define PWDN_GPIO_NUM     32
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM      0
#define SIOD_GPIO_NUM     26
#define SIOC_GPIO_NUM     27
#define Y9_GPIO_NUM       35
#define Y8_GPIO_NUM       34
#define Y7_GPIO_NUM       39
#define Y6_GPIO_NUM       36
#define Y5_GPIO_NUM       21
#define Y4_GPIO_NUM       19
#define Y3_GPIO_NUM       18
#define Y2_GPIO_NUM        5
#define VSYNC_GPIO_NUM    25
#define HREF_GPIO_NUM     23
#define PCLK_GPIO_NUM     22

// Function prototypes
void startCameraServer();
void setupMotors();
void motorStop();
void motorForward();
void motorReverse();

void setup() 
{
  // Prevent brownout errors
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);

  Serial.begin(115200);
  Serial.setDebugOutput(true);
  Serial.println();

  // Camera configuration
  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;

  // Use higher specs if PSRAM is available
  if(psramFound()){
    config.frame_size = FRAMESIZE_QVGA;
    config.jpeg_quality = 10;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_QVGA;
    config.jpeg_quality = 12;
    config.fb_count = 1;
  }

  // Initialize the camera
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
  }

  // Adjust sensor settings for orientation and frame rate
  sensor_t * s = esp_camera_sensor_get();
  s->set_framesize(s, FRAMESIZE_QVGA);
  s->set_vflip(s, 1);
  s->set_hmirror(s, 1);

  // Start the WiFi Access Point
  WiFi.softAP(ssid1, password1);
  IPAddress myIP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(myIP);
  
  // Start the camera server (implementation in app_httpd.cpp or similar)
  startCameraServer();

  // Set up the LED indicator on pin LED_PIN using LEDC
  ledcSetup(7, 5000, 8);  // Channel 7, 5000Hz, 8-bit resolution
  ledcAttachPin(LED_PIN, 7);

  // Set up motor control pins
  setupMotors();
  
  // Flash LED to indicate start-up
  for (int i = 0; i < 5; i++) 
  {
    ledcWrite(7, 10);
    delay(50);
    ledcWrite(7, 0);
    delay(50);    
  }
      
  previous_time = millis();
}

void loop() 
{
  // Here, we perform a sample periodic motor action based on move_interval
  if(robo)
  {
    unsigned long currentMillis = millis();
    if (currentMillis - previous_time >= move_interval) {
      previous_time = currentMillis;
      
      // Stop the motors
      motorStop();
      
      Serial.print("Stopped motors at speed: ");
      Serial.println(motor_speed);
      
      // Reset the trigger (robo becomes inactive)
      robo = 0;
    }
    else {
      // Run motors forward as an example
      motorForward();
    }
  }
  delay(1);
  yield();
}

// Function to initialize motor control pins and set initial values
void setupMotors() {
  // Set all motor control pins as outputs
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  
  // Ensure motors are stopped initially
  motorStop();
}

// Stops both motors by setting all control pins LOW
void motorStop() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

// Drives both motors forward. To change speed control, you might use PWM.
void motorForward() {
  // For forward motion, set IN1 & IN3 HIGH, IN2 & IN4 LOW.
  // Adjust this configuration based on your motor wiring.
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

// Drives both motors in reverse. Adjust pin states if necessary.
void motorReverse() {
  // For reverse motion, set IN1 & IN3 LOW, IN2 & IN4 HIGH.
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

// Stub for starting the camera server. Be sure to implement this as needed.
void startCameraServer() {
  // Typically, you would initialize your HTTP server for streaming here.
  Serial.println("Camera server started (stub).");
}