#include <math.h>
#include "esp_camera.h"
#include "esp_http_server.h"

#ifndef VECTOR_H
#include "vector.h"
#define VECTOR_H
#endif

#define INTENSITY_THRESHOLD 64
#define MOTION_THRESHOLD 100
#define BOUNDARY "1234567890abcdefghij"
#define PWDN_GPIO_NUM -1
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 10
#define SIOD_GPIO_NUM 40
#define SIOC_GPIO_NUM 39
#define Y9_GPIO_NUM 48
#define Y8_GPIO_NUM 11
#define Y7_GPIO_NUM 12
#define Y6_GPIO_NUM 14
#define Y5_GPIO_NUM 16
#define Y4_GPIO_NUM 18
#define Y3_GPIO_NUM 17
#define Y2_GPIO_NUM 15
#define VSYNC_GPIO_NUM 38
#define HREF_GPIO_NUM 47
#define PCLK_GPIO_NUM 13

static camera_config_t camera_config = {
    .pin_pwdn = PWDN_GPIO_NUM,
    .pin_reset = RESET_GPIO_NUM,
    .pin_xclk = XCLK_GPIO_NUM,
    .pin_sscb_sda = SIOD_GPIO_NUM,
    .pin_sscb_scl = SIOC_GPIO_NUM,

    .pin_d7 = Y9_GPIO_NUM,
    .pin_d6 = Y8_GPIO_NUM,
    .pin_d5 = Y7_GPIO_NUM,
    .pin_d4 = Y6_GPIO_NUM,
    .pin_d3 = Y5_GPIO_NUM,
    .pin_d2 = Y4_GPIO_NUM,
    .pin_d1 = Y3_GPIO_NUM,
    .pin_d0 = Y2_GPIO_NUM,
    .pin_vsync = VSYNC_GPIO_NUM,
    .pin_href = HREF_GPIO_NUM,
    .pin_pclk = PCLK_GPIO_NUM,

    // XCLK 20MHz or 10MHz for OV2640 double FPS (Experimental)
    .xclk_freq_hz = 20000000,
    .ledc_timer = LEDC_TIMER_0,
    .ledc_channel = LEDC_CHANNEL_0,

    .pixel_format = PIXFORMAT_RGB565, // YUV422,GRAYSCALE,RGB565,JPEG
    .frame_size = FRAMESIZE_QVGA,     // QQVGA-UXGA Do not use sizes above QVGA when not JPEG

    .jpeg_quality = 12, // 0-63 lower number means higher quality
    .fb_count = 1,      // if more than one, i2s runs in continuous mode. Use only with JPEG
    .fb_location = CAMERA_FB_IN_PSRAM,
    .grab_mode = CAMERA_GRAB_WHEN_EMPTY,
};

void camera_setup()
{
  esp_camera_init(&camera_config);
}

int last_i = -1;
int last_j = -1;

Vector get_light_pos()
{
  camera_fb_t *framebuffer = NULL;
  esp_err_t result = ESP_OK;
  size_t header_length, jpg_length;
  uint8_t *rgb_buffer = NULL;
  uint32_t width, height, len, row, col, intensity, motion;
  //
  // get frame
  //
  framebuffer = esp_camera_fb_get();
  width = framebuffer->width;
  height = framebuffer->height;
  int max_i = 0;
  int max_j = 0;
  int max_val = 0;
  uint16_t *rgb565_buf = (uint16_t *)framebuffer->buf;
  for (int i = 0; i < height; i++)
  {
    for (int j = 0; j < width; j++)
    {
      uint16_t rgb = rgb565_buf[i * width + j];
      uint8_t r = rgb >> 11;
      uint8_t g = rgb >> 5 & (0b111111);
      uint8_t b = rgb & (0b11111);
      if ((r + g + b) > max_val)
      {
        max_val = (r + g + b);
        max_i = i;
        max_j = j;
      }
    }
  }
  last_i = max_i;
  last_j = max_j;

  //    len = width*height*3;
  //    //
  //    // convert to RGB
  //    //
  //    if (!(rgb_buffer))
  //       rgb_buffer = (uint8_t *)ps_malloc(len);
  //    fmt2rgb888(framebuffer->buf,framebuffer->len,PIXFORMAT_JPEG,rgb_buffer);
  esp_camera_fb_return(framebuffer);

  return Vector(max_i, max_j, 0);
}