#include "command.h"

#include <stdint.h>
#include <stdlib.h>
#include <inttypes.h>
#include <stdio.h>
#include <strings.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h>
#include <time.h>

#include "Devices/lmx2594.h"
#include "Devices/dac8811.h"
#include "Devices/ad9912.h"
#include "Devices/potentiometer.h"

double f_pd = 200e6;

uint16_t armCode[1] = {0};
uint16_t attCode[1] = {0};
uint16_t offsetCode[1] = {0};
uint16_t slopeCode[1] = {0};

//Массив структур Command, который связывает строки команд с соответствующими функциями.
Command commands[] = {
	{"TMSG44:FREQ ", 	handleFreqCmd},
	{"TMSG44:LD?", 		handleLdCmd},
	{"TMSG44:POW ", 	handlePowCmd},
	{"TMSG44:ARM ", 	handleArmCmd},
	{"TMSG44:ATT ", 	handleAttCmd},
	{"*IDN?", 			handleIdnCmd},
	{"TMSG44:OFFSET ",	handleOffsetCmd},
	{"TMSG44:SLOPE ",	handleSlopeCmd},
	{NULL, NULL} // Завершающий элемент для обозначения конца массива
};

//handleXXXXCmd - обработчики команд
void handleFreqCmd(const char* recvBuff)
{
	
	printf("\nHandle command \"TMSG44:FREQ\"\n");
	double freq[1] = {0};
	double lmx_freq = 0;

	splitLexeme(recvBuff, freq, sizeof(freq[0]), convertToDouble);
	// usleep(1);
    uint32_t cfg_reg = get_cfg_reg();
    SET_REGISTER_PARAM(cfg_reg, CFG_REG_SPI_MODE_BITM, CFG_REG_SPI_MODE_BITP, CFG_REG_SPI_MODE_4MOSI); 
    uint32_t *spi_mode = bar1 +CFG_REG_ADDR;
    *spi_mode = cfg_reg;

	set_cfg_reg(cfg_reg);
	potentiometer_set_qspi((uint8_t) offsetCode[0], (uint8_t) slopeCode[0]);
	lmx_freq = lmx_get_freq(freq[0]);
	f_pd = ad9912_set(bar1, lmx_freq, f_pd);
	printf("f_pd frequency is set to %.6f MHz\n", f_pd/1e6);
    lmx_freq_set(bar1, lmx_freq, f_pd);
	// Switch the keys 
	key_switch(bar1, freq[0],lmx_freq);
    printf("The frequency is set to %.2f MHz\n", freq[0]/1e6);
	// Send the data 
	send_data_qspi(bar1);
	   // Return the 1 MOSI mode
    // usleep(1);
	// cfg_reg = get_cfg_reg();
    // SET_REGISTER_PARAM(cfg_reg,CFG_REG_SPI_MODE_BITM,CFG_REG_SPI_MODE_BITP, CFG_REG_SPI_MODE_1MOSI);
    // *spi_mode = cfg_reg;
    // set_cfg_reg(cfg_reg);
	
}
void send_data_qspi(void *bar1) {
	// get the gpio reg and shift reg data 
	uint32_t gpio_reg = get_tmsg_gpio_reg();
	uint32_t shift_reg = get_tmsg_shift_reg();
	// Create a header 4 Mosi mode
	uint32_t qspi_header = ((ENUM_SPIMODE_4MOSI) |(0x1 << BITP_GPIO_4MOSI_HEADER) |(0x1 << BITP_SHIFT_REG_4MOSI_HEADER )| ((sizeof(ad9912_ftw_regs_qspi) / 4) << BITP_DDS_4MOSI_HEADER) | ((sizeof(lmx_change_freq_regs) / 4) << BITP_LMX2594_4MOSI_HEADER) | TERM_BIT_1);
	uint32_t *data = bar1 + LMX_BASE_ADDR;
	*data = qspi_header;
	// Initialize the registers
	// Send the data for AD9912
	for (int i = 0; i < sizeof(ad9912_ftw_regs_qspi) / 4; i++) {
		*data = ad9912_ftw_regs_qspi[i];
	}
	// Send the data for the GPIO
	*data = gpio_reg;
	// Send the data for LMX2594
	for (int i = 0; i < sizeof(lmx_change_freq_regs) / 4; i++) {
		*data = lmx_change_freq_regs[i];
	}
	// Send the data for the shift register
	*data = shift_reg;

}

void handleLdCmd(const char* recvBuff)
{
	char messageLd[] = "1\n";

	printf("\nHandle command \"TMSG44:LD?\"\n");

	uint32_t ld_status = lmx_ld_status(bar1);

	clock_t before = clock();
	clock_t difference;
	int difference_msec = 0;
	int trigger = 10000; //10ms

	while(!ld_status)
	{
		difference = clock() - before;
		difference_msec = difference * 1000 / CLOCKS_PER_SEC;

		if(difference_msec >= trigger)
		{
			strcpy(messageLd, "0\n");
			printf("LD timeout\n");
			break;
		}
		ld_status = lmx_ld_status(bar1);
		// printf("WHILE LD status: %d\n", ld_status);
	}

	send(conn_fd, messageLd, sizeof(messageLd), 0);
	
	printf("\nSend msg LD: %d!\n", ld_status);
}

void handlePowCmd(const char* recvBuff)
{
	printf("\nHandle command \"TMSG44:POW\"\n");
	double pow[1] = {0};

	splitLexeme(recvBuff, pow, sizeof(pow[0]), convertToDouble);
	printf("%f\n", pow[0]);
}

void handleArmCmd(const char* recvBuff)
{
	printf("\nHandle command \"TMSG44:ARM\"\n");

	splitLexeme(recvBuff, armCode, sizeof(armCode[0]), convertToUInt16);
	printf("\n%u\n", armCode[0]);

	uint32_t cfg_reg = get_cfg_reg();
    SET_REGISTER_PARAM(cfg_reg, CFG_REG_SPI_MODE_BITM, CFG_REG_SPI_MODE_BITP, CFG_REG_SPI_MODE_4MOSI); 
    uint32_t *spi_mode = bar1 +CFG_REG_ADDR;
    *spi_mode = cfg_reg;
	set_cfg_reg(cfg_reg);

	dac8811_set_qspi(bar1,armCode[0]);
}

void handleAttCmd(const char* recvBuff)
{
	printf("\nHandle command \"TMSG44:ATT\"\n");

	splitLexeme(recvBuff, attCode, sizeof(attCode[0]), convertToUInt16);
	printf("\n%u\n", attCode[0]);

	uint32_t cfg_reg = get_cfg_reg();
    SET_REGISTER_PARAM(cfg_reg, CFG_REG_SPI_MODE_BITM, CFG_REG_SPI_MODE_BITP, CFG_REG_SPI_MODE_4MOSI); 
    uint32_t *spi_mode = bar1 +CFG_REG_ADDR;
    *spi_mode = cfg_reg;
	set_cfg_reg(cfg_reg);

	dac8811_att_set_qspi(bar1, attCode[0]);
}

void handleIdnCmd(const char* recvBuff)
{
	printf("\nHandle command \"*IDN?\"\n");

	char messageIdn[] = "TMSG44_CoolPi\n";
	send(conn_fd, messageIdn, sizeof(messageIdn), 0);
}

void handleOffsetCmd(const char* recvBuff)
{
	printf("\nHandle command \"TMSG44:OFFSET\"\n");

	splitLexeme(recvBuff, offsetCode, sizeof(offsetCode[0]), convertToUInt16);
	printf("\n%u\n", offsetCode[0]);
	uint32_t cfg_reg = get_cfg_reg();
    SET_REGISTER_PARAM(cfg_reg, CFG_REG_SPI_MODE_BITM, CFG_REG_SPI_MODE_BITP, CFG_REG_SPI_MODE_4MOSI); 
    uint32_t *spi_mode = bar1 +CFG_REG_ADDR;
    *spi_mode = cfg_reg;
	set_cfg_reg(cfg_reg);
	potentiometer_set_offset(bar1, offsetCode[0]);
}

void handleSlopeCmd(const char* recvBuff)
{
	printf("\nHandle command \"TMSG44:SLOPE\"\n");
	splitLexeme(recvBuff, slopeCode, sizeof(slopeCode[0]), convertToUInt16);
	printf("\n%u\n", slopeCode[0]);
	uint32_t cfg_reg = get_cfg_reg();
    SET_REGISTER_PARAM(cfg_reg, CFG_REG_SPI_MODE_BITM, CFG_REG_SPI_MODE_BITP, CFG_REG_SPI_MODE_4MOSI); 
    uint32_t *spi_mode = bar1 +CFG_REG_ADDR;
    *spi_mode = cfg_reg;
	set_cfg_reg(cfg_reg);
	potentiometer_set_slope(bar1, slopeCode[0]);
}

//Проходим по массиву команд и ищем команду, которая совпадает с началом строки recvBuff. 
//Если команда найдена, вызывается соответствующая функция-обработчик
void processCommand(const char* recvBuff) 
{
	for (int i = 0; commands[i].command != NULL; i++) 
	{
		if (!strncasecmp(recvBuff, commands[i].command, strlen(commands[i].command))) 
		{
			commands[i].handler(recvBuff);
			return;
		}
	}
	printf("\nUnknown command: %s\n", recvBuff);
}

// Преобразование строки в uint16_t
void convertToUInt16(const char *str, void *output) 
{
	*(uint16_t *)output = (uint16_t)strtoul(str, NULL, 10);
}

// Преобразование строки в unsigned long long int
void convertToUint64(const char *str, void *output) 
{
	*(uint64_t *)output = (uint64_t)strtoull(str, NULL, 10);
}

// Преобразование строки в double
void convertToDouble(const char *str, void *output) 
{
	*(double *)output = strtod(str, NULL);
}

// Универсальная функция для разделения строки на лексемы
void splitLexeme(const char *ptrSCPI, void *numOutAndValue, size_t elementSize, ConvertFunc convertFunc) 
{
	uint8_t counter = 0;

	// Разделители лексем
	const char charSeparator[] = {" "};
	char *ptrLexeme = NULL;
	// Указатель для хранения контекста токенизации
	char *savePtr;

	// Инициализируем функцию
	ptrLexeme = strtok_r((char *)ptrSCPI, charSeparator, &savePtr);

	// Ищем лексемы разделенные разделителем
	ptrLexeme = strtok_r(NULL, charSeparator, &savePtr);

	// Ищем лексемы строки
	while (ptrLexeme) {
		// Проверяем, является ли первый символ лексемы числом
		if(('0' <= ptrLexeme[0]) && (ptrLexeme[0] <= '9')) {
			// Преобразуем строку с числом в число
			convertFunc(ptrLexeme, (uint8_t *)numOutAndValue + counter * elementSize);
			counter++;
		}
		// Ищем лексемы разделенные разделителем
		ptrLexeme = strtok_r(NULL, charSeparator, &savePtr);
	}
}