/*********
  Rui Santos
  Complete project details at https://RandomNerdTutorials.com/esp32-cam-video-streaming-web-server-camera-home-assistant/

  IMPORTANT!!!
   - Select Board "AI Thinker ESP32-CAM"
   - GPIO 0 must be connected to GND to upload a sketch
   - After connecting GPIO 0 to GND, press the ESP32-CAM on-board RESET button to put your board in flashing mode

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files.

  The above copyright notice and this permission notice shall be included in all
  copies or substantial portions of the Software.
*********/

/*
PCA9685 goes onto pins 15(SDA) and 14(SCL) of the esp32-cam. it will create a wifi hotspot on startup.
192.168.4.1 will have the camera feed (but as of now it blocks the rest of the program from running).
You add frames by POSTing them in binary format to 192.168.4.1/sv:

    requests.post("http://192.168.4.1/sv", data=bytes([0x07, 0x01, 0x00, 0x1A, 0x00, 0x2A]))

the first byte selects the servo. the second byte is the flag bits, these are set to 0x01 for normal move,
0xFE to skip this servo or 0xFF to force-stop. the next two bytes are the position in degrees,
and the last two are the speed in deg/s, both as 16 bit unsigned integers.

this will only load the movement into the buffer, to play it back send data=bytes([0xFF]).
You can send as few or as many servo commands as you want in a single request, up to the FRAME_SZ.
However, you must send 0xFF on its own (it is a substitute for an external trigger signal on PIN 2).
*/

#define DEBUG2

#include "esp_camera.h"
#include <WiFi.h>
#include "esp_timer.h"
#include "img_converters.h"
#include "Arduino.h"
#include "fb_gfx.h"
#include "soc/soc.h" //disable brownout problems
#include "soc/rtc_cntl_reg.h"  //disable brownout problems
#include "esp_http_server.h"
#include "driver/i2c.h"

// ***WIFI/SERVER VARIABLES***
const char* ssid     = "ESP32-Access-Point";
const char* password = "123456789";

#define PART_BOUNDARY "123456789000000000000987654321"

// This project was tested with the AI Thinker Model, M5STACK PSRAM Model and M5STACK WITHOUT PSRAM
#define CAMERA_MODEL_AI_THINKER

#if defined(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
#else
  #error "Camera model not selected"
#endif

static const char* _STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char* _STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char* _STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";

httpd_handle_t stream_httpd = NULL;

// ***BUFFER VARIABLES***
#define BUFFER_EMPTY 1
#define BUFFER_FULL 2
#define BUFFER_SUCCESS 0
#define BUFFER_SZ 10 // in frames
#define FRAME_SZ 192 // in bytes; set to servo count * 6

struct frame_buffer_t {
    int read_ptr;
    int write_ptr;
    int items;
    char *contents[BUFFER_SZ];
};

frame_buffer_t framebuf;

// ***I2C VARIABLES***
#define I2C_SCL GPIO_NUM_15
#define I2C_SDA GPIO_NUM_14

// i2c device addresses
#define SVDRIVER1 0x40
#define SVDRIVER2 0x40 //TODO
#define IMU       0x68

// servo driver stuff
#define DRV_PRESCALE_VAL 122
#define DRV_MODE     0x00
#define DRV_SLEEP    0x10
#define DRV_RESTART  0x80
#define DRV_PRESCALE 0xFE
#define DRV_ON_PTR   0x06
#define DRV_RESTART  0x80

// ***SERVO CONTROL VARIABLES***
#define SERVO_COUNT 10

struct servo_command_t {
    uint8_t id;
    uint8_t flags;
    uint16_t position;
    uint16_t velocity;
};

struct servo_action_t {
    uint8_t driver_id;
    uint16_t last_pos;
    uint16_t goal_pos;
    uint16_t real_pos;
    int speed;
    unsigned long cycle_dur;
    unsigned long cycle_start;
    bool moving;
};

servo_action_t servos[SERVO_COUNT];

int frames_to_play = 0;

// ***BUFFER SECTION***

void buffer_init() {
    framebuf.read_ptr = 0;
    framebuf.write_ptr = 0;
    framebuf.items = 0;

    for (int i = 0; i < BUFFER_SZ; i++) {
        char *buf_item = new char[FRAME_SZ];
        memset(buf_item, 0, FRAME_SZ);
        framebuf.contents[i] = buf_item;
    }
}

int buffer_add(char* frame) {
    if (framebuf.items >= BUFFER_SZ)
        return BUFFER_FULL;

    memcpy(framebuf.contents[framebuf.write_ptr], frame, FRAME_SZ);

    framebuf.items++;
    framebuf.write_ptr++;
    if (framebuf.write_ptr >= BUFFER_SZ)
        framebuf.write_ptr = 0;

    return BUFFER_SUCCESS;
}

// sets frame to the first full buffer item
int buffer_get(char* frame) {
    if (framebuf.items == 0)
        return BUFFER_EMPTY;

    memcpy(frame, framebuf.contents[framebuf.read_ptr], FRAME_SZ);

    framebuf.items--;
    framebuf.read_ptr++;
    if (framebuf.read_ptr >= BUFFER_SZ)
        framebuf.read_ptr = 0;

    return BUFFER_SUCCESS;
}

#ifdef DEBUG2
void print_frame(char* frame) {
    yield();
    char temp[(FRAME_SZ * 2) + 1];
    for (int i = 0; i < FRAME_SZ; i++) {
        char conv[3];
        sprintf(conv, "%02X", frame[i]);
        temp[i * 2] = conv[0];
        temp[(i * 2) + 1] = conv[1];
    }
    Serial.print("DEBUG: Frame print: ");
    Serial.println(temp);
}

void print_all_frames() {
    for (int i = 0; i < BUFFER_SZ; i++) {
        Serial.print("DEBUG: frame ");
        Serial.println(i);
        print_frame(framebuf.contents[i]);
    }
}
#endif

// ***I2C SECTION***

void i2c_init() {
    i2c_config_t conf;
    conf.mode = I2C_MODE_MASTER;
    conf.sda_io_num = I2C_SDA;
    conf.sda_pullup_en = GPIO_PULLUP_DISABLE;
    conf.scl_io_num = I2C_SCL;
    conf.scl_pullup_en = GPIO_PULLUP_DISABLE;
    conf.master.clk_speed = 100000;

    i2c_param_config(I2C_NUM_1, &conf);
    i2c_driver_install(I2C_NUM_1, conf.mode, 0, 0, 0);
}

// write len bytes from *dat to dev
esp_err_t i2c_write(uint8_t dev, uint8_t *dat, size_t len) {
    i2c_cmd_handle_t i2c_h = i2c_cmd_link_create();

    i2c_master_start(i2c_h);
    i2c_master_write_byte(i2c_h, (dev << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(i2c_h, dat, len, true);
    i2c_master_stop(i2c_h);

    esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_1, i2c_h, (1000 / portTICK_RATE_MS));
    i2c_cmd_link_delete(i2c_h);

    return ret;
}

// read len bytes from dev's reg into *dat
esp_err_t i2c_read(uint8_t dev, uint8_t reg, uint8_t *dat, size_t len) {
    i2c_write(dev, &reg, 1); //set register

    i2c_cmd_handle_t i2c_h = i2c_cmd_link_create();

    i2c_master_start(i2c_h);
    i2c_master_write_byte(i2c_h, (dev << 1) | I2C_MASTER_READ, true);
    i2c_master_read(i2c_h, dat, len, I2C_MASTER_ACK);
    i2c_master_stop(i2c_h);

    esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_1, i2c_h, (1000 / portTICK_RATE_MS));
    i2c_cmd_link_delete(i2c_h);

    return ret;
}

void drv_init(uint8_t id) {
    uint8_t curr_mode;
    uint8_t cmd[2];

    // reset
    cmd[0] = DRV_MODE;
    cmd[1] = DRV_RESTART;
    i2c_write(id, cmd, 2);
    delay(10);

    // shut off driver
    i2c_read(id, DRV_MODE, &curr_mode, 1);
    cmd[0] = DRV_MODE;
    cmd[1] = (curr_mode & ~DRV_RESTART) | DRV_SLEEP;
    i2c_write(id, cmd, 2);

    // set prescale
    cmd[0] = DRV_PRESCALE;
    cmd[1] = (uint8_t)DRV_PRESCALE_VAL;
    i2c_write(id, cmd, 2);

    // restart driver
    cmd[0] = DRV_MODE;
    cmd[1] = curr_mode | DRV_RESTART | 0x20;
    i2c_write(id, cmd, 2);
    delay(10);
}

void drv_write(uint8_t id, uint8_t pin, uint16_t amt) {
    uint8_t dat[5];
    dat[0] = DRV_ON_PTR + (4 * pin); // pin register pointer
    dat[1] = 0x00; // start pos = 0
    dat[2] = 0x00;
    dat[3] = amt; // end pos high
    dat[4] = amt >> 8; // end pos low
    i2c_write(id, dat, 5);
}

// ***SERVO CONTROL SECTION***

// dat = charstream from POST request/buffer
// index = command location in stream
// command format: [index][flag][posn_high][posn_low][vel_high][vel_low]
servo_command_t get_servo_command(char *dat, size_t index) {
    size_t posn = (index * 6);

    uint8_t servo_index = dat[posn];
    uint8_t servo_flags = dat[1 + posn];
    uint16_t pos = ((uint16_t)dat[2 + posn] << 8) | ((uint16_t)dat[3 + posn]);
    uint16_t vel = ((uint16_t)dat[4 + posn] << 8) | ((uint16_t)dat[5 + posn]);

    servo_command_t cmd;
    cmd.id = servo_index;
    cmd.flags = servo_flags;
    cmd.position = pos;
    cmd.velocity = vel;

    return cmd;
}

// takes a desired servo move and sets all the
// vars as necessary - call this to move a servo
void set_servo(servo_command_t cmd) {
    servos[cmd.id].driver_id = cmd.id;
    servos[cmd.id].last_pos = servos[cmd.id].real_pos; // save current position
    servos[cmd.id].goal_pos = cmd.position;

    switch (cmd.flags) {
    case 254: // disable
        servos[cmd.id].moving = false;
    case 255: // skip
        return;
        break;
    }

    // record start time and set velocity
    servos[cmd.id].cycle_start = millis();
    uint32_t pos_delta = abs(servos[cmd.id].goal_pos - servos[cmd.id].last_pos);

    if (pos_delta != 0 && cmd.velocity != 0) {
        servos[cmd.id].cycle_dur = ((pos_delta * 1000) / cmd.velocity);
        servos[cmd.id].moving = true;
    }
    else {
        servos[cmd.id].moving = false;
    }
}

uint16_t set_from_degrees(uint8_t id, uint16_t deg, uint8_t scale) {
    //TODO: using random min/max values, provide a way to calibrate
    uint8_t driver = id ? SVDRIVER2 : SVDRIVER1;
    uint8_t hw_pin = id % 16; // select second driver if needed
    uint16_t pwm_on_time = map((deg / scale), 0, 180, 205, 410); // 1/20 to 2/20 pwm
    drv_write(driver, hw_pin, pwm_on_time);
}

// play back a frame
void set_servos_from_frame(char *frame, size_t frame_len) {
#ifdef DEBUG2
    print_all_frames();
    Serial.print("DEBUG: setting frame: ");
    print_frame(frame);
#endif
    for (int i = 0; i < frame_len / 6; i++) {
        servo_command_t cmd = get_servo_command(frame, i);
        if (!cmd.flags)
            break;
#ifdef DEBUG2
        Serial.print("DEBUG: setting servo ");
        Serial.print(cmd.id);
        Serial.print(" with position ");
        Serial.print(cmd.position);
        Serial.print(" velocity ");
        Serial.print(cmd.velocity);
        Serial.print(" flags ");
        Serial.println(cmd.flags);
#endif
        set_servo(cmd);
    }
    memset(frame, 0, FRAME_SZ);
}

void update_servos() {
    // check for unhandled frame pulses
    if (frames_to_play) {
        char frame[FRAME_SZ];

        if (buffer_get(frame) == BUFFER_EMPTY) {
#ifdef DEBUG2
            Serial.println("DEBUG: Frame requested but no frame in buffer");
#endif
        }
        else {
            set_servos_from_frame(frame, FRAME_SZ);
#ifdef DEBUG2
            Serial.print("DEBUG: ");
            Serial.print(framebuf.items);
            Serial.println(" left in buffer.");
#endif
        }
        frames_to_play--;
    }

    // update actual servo positions based on set timers
    for (int i = 0; i < SERVO_COUNT; i++) {
        if (servos[i].cycle_dur != 0) {
            unsigned long time_from_move_start = millis() - servos[i].cycle_start;

            if (servos[i].moving) {
                servos[i].real_pos = map(
                                         time_from_move_start,
                                         0,
                                         servos[i].cycle_dur - 1,
                                         servos[i].last_pos,
                                         servos[i].goal_pos);

                set_from_degrees(servos[i].driver_id, servos[i].real_pos, 1);
            }

            if (time_from_move_start >= servos[i].cycle_dur) {
                servos[i].moving = false;
                servos[i].last_pos = servos[i].real_pos;
                servos[i].real_pos = servos[i].goal_pos;
                set_from_degrees(servos[i].driver_id, servos[i].real_pos, 1);
            }
        }
    }
}

// ***WEBSERVER SECTION***

//TODO: this will call set_servo/set_servos_from_cmds
// from esp-idf example code
esp_err_t servo_req_handler(httpd_req_t *req) {
    char content[FRAME_SZ];
    char res_text[64];

    /* Truncate if content length larger than the buffer */
    memset(content, 0, FRAME_SZ);
    size_t recv_size = min(req->content_len, sizeof(content));

    int ret = httpd_req_recv(req, content, recv_size);
    if (ret <= 0) {  /* 0 return value indicates connection closed */
        /* Check if timeout occurred */
        if (ret == HTTPD_SOCK_ERR_TIMEOUT) {
            /* In case of timeout one can choose to retry calling
             * httpd_req_recv(), but to keep it simple, here we
             * respond with an HTTP 408 (Request Timeout) error */
            httpd_resp_send_408(req);
        }
        /* In case of error, returning ESP_FAIL will
         * ensure that the underlying socket is closed */
        return ESP_FAIL;
    }

#ifdef DEBUG2
    Serial.println("DEBUG: received message");
    Serial.println(content);
    Serial.println(recv_size);
#endif

    // data received multiple of frame len - frame input
    if (!(recv_size % 6)) {
        if (buffer_add(content) == BUFFER_FULL)
            sprintf(res_text, "E: buffer full");
        else
            sprintf(res_text, "OK");
    }
    else if (recv_size < 6) {
        if (content[0] == 0xFF)
            frames_to_play++;

        sprintf(res_text, "TODO: cmds here");
    }
    else {
        sprintf(res_text, "E: bad input");
    }

    sprintf(res_text + strlen(res_text), "buffer status: %d / %d", framebuf.items, BUFFER_SZ);
    httpd_resp_send(req, res_text, strlen(res_text));
#ifdef DEBUG2
    Serial.println(res_text);
    print_all_frames();
#endif
    return ESP_OK;
}

void IRAM_ATTR on_adv_pulse(void* arg) {
    frames_to_play++;
}

// camera server code from RandomNerdTutorials
//TODO:
static esp_err_t stream_handler(httpd_req_t *req){
    camera_fb_t * fb = NULL;
    esp_err_t res = ESP_OK;
    size_t _jpg_buf_len = 0;
    uint8_t * _jpg_buf = NULL;
    char * part_buf[64];

    res = httpd_resp_set_type(req, _STREAM_CONTENT_TYPE);
    if(res != ESP_OK){
        return res;
    }

    while(true){
        fb = esp_camera_fb_get();
        if (!fb) {
            Serial.println("Camera capture failed");
            res = ESP_FAIL;
        } else {
            if(fb->width > 400){
                if(fb->format != PIXFORMAT_JPEG){
                    bool jpeg_converted = frame2jpg(fb, 80, &_jpg_buf, &_jpg_buf_len);
                    esp_camera_fb_return(fb);
                    fb = NULL;
                    if(!jpeg_converted){
                        Serial.println("JPEG compression failed");
                        res = ESP_FAIL;
                    }
                } else {
                    _jpg_buf_len = fb->len;
                    _jpg_buf = fb->buf;
                }
            }
        }
        if(res == ESP_OK){
            size_t hlen = snprintf((char *)part_buf, 64, _STREAM_PART, _jpg_buf_len);
            res = httpd_resp_send_chunk(req, (const char *)part_buf, hlen);
        }
        if(res == ESP_OK){
            res = httpd_resp_send_chunk(req, (const char *)_jpg_buf, _jpg_buf_len);
        }
        if(res == ESP_OK){
            res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
        }
        if(fb){
            esp_camera_fb_return(fb);
            fb = NULL;
            _jpg_buf = NULL;
        } else if(_jpg_buf){
            free(_jpg_buf);
            _jpg_buf = NULL;
        }
        if(res != ESP_OK){
            break;
        }
    }
    return res;
}

void startCameraServer() {
    httpd_config_t config = HTTPD_DEFAULT_CONFIG();
    config.server_port = 80;

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

    httpd_uri_t servo_uri = {
        .uri         = "/sv",
        .method      = HTTP_POST,
        .handler     = servo_req_handler,
        .user_ctx    = NULL
    };

    //Serial.printf("Starting web server on port: '%d'\n", config.server_port);
    if (httpd_start(&stream_httpd, &config) == ESP_OK) {
        httpd_register_uri_handler(stream_httpd, &index_uri);
        httpd_register_uri_handler(stream_httpd, &servo_uri);
    }
}

void setup() {
    WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector

    Serial.begin(115200);
    Serial.setDebugOutput(false);

    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_UXGA;
        config.jpeg_quality = 10;
        config.fb_count = 2;
    } else {
        config.frame_size = FRAMESIZE_SVGA;
        config.jpeg_quality = 12;
        config.fb_count = 1;
    }

    // Camera init
    esp_err_t err = esp_camera_init(&config);
    if (err != ESP_OK) {
        Serial.printf("Camera init failed with error 0x%x", err);
        return;
    }
    // Connect to Wi-Fi network with SSID and password
    Serial.print("Setting AP (Access Point)…");
    // Remove the password parameter, if you want the AP (Access Point) to be open
    WiFi.softAP(ssid, password);

    IPAddress IP = WiFi.softAPIP();
    Serial.print("Camera Stream Ready! Connect to the ESP32 AP and go to: http://");
    Serial.println(IP);

    // Start streaming web server
    startCameraServer();

    buffer_init();
    i2c_init();
    drv_init(SVDRIVER1);

    gpio_set_intr_type(GPIO_NUM_2, GPIO_INTR_POSEDGE);
    gpio_install_isr_service(0);
    gpio_isr_handler_add(GPIO_NUM_2, on_adv_pulse, NULL);

#ifdef DEBUG2
    print_all_frames();
#endif
}

void loop() {
    delay(1);
    update_servos();
}