#include <stdbool.h>
#include "esp_system.h"

#include "src/lipton/cci.h"
#include "src/lipton/vospi.h"
#include "src/lipton/lepton_system.h"
#include "src/lipton/lepton_utilities.h"


#include "mbedtls/base64.h"


//
// LEP Task constants
//

// Number of consecutive VoSPI resynchronization attempts before attempting to reset
#define LEP_SYNC_FAIL_FAULT_LIMIT 10

// Reset fail delay before attempting a re-init (seconds)
#define LEP_RESET_FAIL_RETRY_SECS 5


//
// picture functions
//
char* base64_encode_lep_image(uint16_t* lep_bufferP, size_t* base64_length) {
  size_t base64_obj_len = 0;
  char* base64_lep_data = NULL;

  // Get the necessary length for base64 encoding
  mbedtls_base64_encode(NULL, 0, &base64_obj_len, (const unsigned char*)lep_bufferP, LEP_NUM_PIXELS * 2);

  // Allocate memory for the base64 encoded data
  base64_lep_data = (char*)malloc(base64_obj_len);
  if (base64_lep_data == NULL) {
    return NULL;  // Memory allocation failed
  }

  // Base64 encode the camera data
  if (mbedtls_base64_encode((unsigned char*)base64_lep_data, base64_obj_len, &base64_obj_len, (const unsigned char*)lep_bufferP, LEP_NUM_PIXELS * 2) != 0) {
    free(base64_lep_data);
    return NULL;  // Encoding failed
  }

  *base64_length = base64_obj_len;

  return base64_lep_data;
}

void pic_to_uart(uint16_t* pic_buf) {
  size_t base64_length = 0;
  char* base64_encoded_data = base64_encode_lep_image(pic_buf, &base64_length);

  if (base64_encoded_data != NULL) {
    // Output the base64 encoded string and its length
    printf("data:base64,%s\n", base64_encoded_data);

    // Remember to free the allocated memory
    free(base64_encoded_data);
  }
}

//
// Code start
//
int vsync_count = 0;
int sync_fail_count = 0;
int reset_fail_count = 0;
int64_t vsyncDetectedUsec;

void setup() {
  printf("[MAIN] Start task\n");

  LEP_CONFIG = {
    .agc_set_enabled = false,
    .emissivity = 100,
    .gain_mode = LEP_GAIN_AUTO
  };

  // initialize spi, i2c and gpio for lepton
  if (!lepton_io_init()) {
    printf("[MAIN] Error: I/O init failed");
    while(1) {delay(100);}
  }

  // allocate lepton buffers
  if (!lepton_buffer_init()) {
    printf("[MAIN] Error: Memory init failed");
    while(1) {delay(100);}
  }

  // wait for lepton to initialize
  delay(1000);

  // Attempt to initialize the VoSPI interface
  if (vospi_init() != ESP_OK) {
    printf("[MAIN] Error: Lepton VoSPI initialization failed\n");
    while(1) {delay(100);}
  }

  // initialize lepton
  if (!lepton_init()) {
    printf("[MAIN] Error: Lepton CCI initialization failed\n");
    while(1) {delay(100);}
  }

  // disable data from lepton
  digitalWrite(LEP_CSN_PIN, 1);
}

uint16_t* take_picture() {
  digitalWrite(LEP_CSN_PIN, 0);
  delay(10);

  while (1) {
    // Spin waiting for vsync to be asserted
    int t = 0;
    while (digitalRead(LEP_VSYNC_PIN) == 0 && t++ < 500) {}

    vsyncDetectedUsec = esp_timer_get_time();

    // Attempt to process a segment
    if (t >= 500 || vospi_transfer_segment(vsyncDetectedUsec)) {
      // Got image
      vsync_count = 0;

      // Copy the frame to the current half of the shared buffer and let rsp_task know
      vospi_get_frame(&rsp_lep_buffer);

      // Hold fault counters reset while operating
      sync_fail_count = 0;

      // disable lepton again
      digitalWrite(LEP_CSN_PIN, 1);
      return rsp_lep_buffer.lep_bufferP;
    } else {
      if (++vsync_count >= 36) {
        vsync_count = 0;
        printf("[MAIN] Could not get lepton image\n");

        delay(185);

        if (sync_fail_count++ == LEP_SYNC_FAIL_FAULT_LIMIT) {
          return nullptr;
        }
      }
    }
  }

  return nullptr;
}

void lepton_restart() {
  while (1) {
    delay(LEP_RESET_FAIL_RETRY_SECS*1000);
    
    // Assert hardware reset
    digitalWrite(LEP_RESET_PIN, LEP_RESET_ON);
    delay(10);
    digitalWrite(LEP_RESET_PIN, LEP_RESET_OFF);


    // Delay for Lepton internal initialization (max 950 mSec)
    delay(1000);

    // if successfully initialized, break out
    if (lepton_init())
      break;
  }
}

void loop() {
  delay(5000);

  uint16_t* picture = take_picture();
  if (picture) {
    pic_to_uart(picture);
  } else {
    lepton_restart();
  }
}