#include "esp_system.h"
#include <stdlib.h>
#include "driver/spi_master.h"
#include "driver/spi_slave.h"
#include "lepton_system.h"
#include "lepton_utilities.h"
#include "i2c.h"
#include "vospi.h"
#include <cstring>


// lepton image and telem buffer
lep_buffer_t rsp_lep_buffer;

/**
 * Initialize the ESP32 GPIO and internal peripherals
 */
bool lepton_io_init()
{
	esp_err_t ret;
	spi_bus_config_t spi_buscfg;
	
	printf("[LEP SYS] Lepton I/O Initialization\n");	
	
	// Attempt to initialize the I2C Master
	ret = i2c_master_init(I2C_MASTER_SCL_PIN, I2C_MASTER_SDA_PIN);
	if (ret != ESP_OK) {
		printf("[LEP SYS] Error: I2C Master initialization failed\n");
		return false;
	}
	
	// Attempt to initialize the SPI Master used by the lep_task
	memset(&spi_buscfg, 0, sizeof(spi_bus_config_t));
	spi_buscfg.miso_io_num=LEP_MISO_PIN;
	spi_buscfg.mosi_io_num=-1;
	spi_buscfg.sclk_io_num=LEP_SCK_PIN;
	spi_buscfg.max_transfer_sz=LEP_PKT_LENGTH;
	spi_buscfg.quadwp_io_num=-1;
	spi_buscfg.quadhd_io_num=-1;
	
	if (spi_bus_initialize(LEP_SPI_HOST, &spi_buscfg, LEP_DMA_NUM) != ESP_OK) {
		printf("[LEP SYS] Error: Lepton SPI initialization failed\n");
		return false;
	}

	// initialize GPIO pins
	gpio_set_direction(LEP_VSYNC_PIN, GPIO_MODE_INPUT);
	#ifdef LEP_RESET_PIN
	gpio_set_direction(LEP_RESET_PIN, GPIO_MODE_OUTPUT);
	gpio_set_level(LEP_RESET_PIN, LEP_RESET_ON);
	vTaskDelay(pdMS_TO_TICKS(10));
	gpio_set_level(LEP_RESET_PIN, LEP_RESET_OFF);
	#endif
	
	return true;
}


/**
 * Allocate lepton buffers on heap
 */
bool lepton_buffer_init()
{
	printf("[LEP SYS] Buffer Allocation\n");
	
	// Allocate the LEP/RSP task lepton frame and telemetry ping-pong buffers
	rsp_lep_buffer.lep_bufferP = (uint8_t*)malloc(LEP_NUM_PIXELS*sizeof(uint8_t));
	if (rsp_lep_buffer.lep_bufferP == NULL) {
		printf("[LEP SYS] Error: malloc RSP lepton shared image buffer failed\n");
		return false;
	}

	rsp_lep_buffer.lep_telemP = (uint16_t*)malloc(LEP_TEL_WORDS*sizeof(uint16_t));
	if (rsp_lep_buffer.lep_telemP == NULL) {
		printf("[LEP SYS] Error: malloc RSP lepton shared telemetry buffer failed\n");
		return false;
	}
	
	return true;
}