#include <stdio.h>
#include <stdbool.h>
#include <stdint.h>

#include <nvs_flash.h>
#include <esp_task_wdt.h>
#include <freertos/idf_additions.h>

#include <CLI/CLI.h>
#include <CMDList/CMDList.h>

#include "config.h"

#include "utils.h"
#include "commands.h"
#include "wifi.h"
#include "logger.h"
#include "ws.h"
#if TARGET == TARGET_TX
#include "led.h"
#include "adc.h"
#elif TARGET == TARGET_RX
#include "servos.h"
#endif

bool volatile running = true;
CMDList_t* cmdList;

bool rxBuffer_overflow = false;

typedef enum {
	STATE_WIFI_CONNECTING,
	STATE_WIFI_WAIT_CONNECTION,
#if TARGET == TARGET_RX
	STATE_WS_CONNECTING,
	STATE_WS_WAIT_CONNECTION,
	STATE_IDEL,
	STATE_DRIVING
#elif TARGET == TARGET_TX
	STATE_WIFI_CONNECTED,
#endif
} MainState_t;
MainState_t MainState = STATE_WIFI_CONNECTING;

#if TARGET == TARGET_TX
typedef enum {
	STATE_WS_CONNECTING,
	STATE_WS_WAIT_CONNECTION,
	STATE_IDEL,
	STATE_DRIVING
} WSState_t;
WSState_t WSStates[3] = {STATE_WS_CONNECTING, STATE_WS_CONNECTING, STATE_WS_CONNECTING};
uint8_t WS_conn_proc = 0;
#endif

#if TARGET == TARGET_RX
ws_client_t ws_client;
#elif TARGET == TARGET_TX
ws_client_t ws_client[3];
#endif

int charOut_uart(const char* c)
{
	printf("%c", *c);
	return 0;
}

#if TARGET == TARGET_RX
int charOut_ws(const char* c)
{
	ws_putchar(ws_client, *c);
	return 0;
}
#endif

void app_main() {
	// disable watchdog
	ESP_ERROR_CHECK(esp_task_wdt_deinit());

	/* Initialize NVS — it is used to store PHY calibration data */
	esp_err_t ret = nvs_flash_init();
	if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
	{
		ESP_ERROR_CHECK(nvs_flash_erase());
		ret = nvs_flash_init();
	}
	ESP_ERROR_CHECK(ret);

	printChipInfo();

#if TARGET == TARGET_TX
	led_init();
	led_setRGB(0, 0, 20);
#elif TARGET == TARGET_RX
	servo_init();
#endif

	wifiInit();
	bool adc_inited = false;
	uint8_t adc_counter = 0;

	running = true;

	// init cli
	cmdList = getCMDList();
	CLI_t cli_uart = CLI_init((CLI_charOutFn)&charOut_uart, cmdList);

	uint8_t charIn = 0;
	EventBits_t bits;
	MainState_t lastMainState = MainState;
#if TARGET == TARGET_RX
	CLI_t cli_ws_client;
#elif TARGET == TARGET_TX
   CLI_t cli_ws_client[3];
#endif

	LOG_D("main: main loop starting in state %d", MainState);
	while (running)
	{
		charIn = getchar();
		if (charIn != 255)
		{
			CLI_charIn(&cli_uart, charIn);
		}

		if (lastMainState != MainState)
		{
			LOG_D("main: MainState changed from %d to %d", lastMainState, MainState);
			lastMainState = MainState;
		}

		switch (MainState)
		{
			case STATE_WIFI_CONNECTING:
				wifi_connect();
				MainState = STATE_WIFI_WAIT_CONNECTION;
			   break;
			case STATE_WIFI_WAIT_CONNECTION:
				bits = xEventGroupGetBits(s_wifi_event_group);
				if ((bits & WIFI_CONNECTED_BIT) != 0)
				{
#if TARGET == TARGET_TX
					led_setRGB(0, 20, 0);
					MainState = STATE_WIFI_CONNECTED;
#elif TARGET == TARGET_RX
					MainState = STATE_WS_CONNECTING;
#endif
					LOG_I("main: connected to ap SSID '%s'", WIFI_SSID);
				}
				else if ((bits & WIFI_FAIL_BIT) != 0)
				{
#if TARGET == TARGET_TX
					led_setRGB(2, 0, 0);
#endif
					MainState = STATE_WIFI_CONNECTING;
					// wifi_reconnect();
					LOG_E("main: Failed to connect to SSID '%s'", WIFI_SSID);
				}
				else if (bits != 0)
				{
#if TARGET == TARGET_TX
					led_setRGB(20, 0, 0);
#endif
					// MainState = STATE_WIFI_CONNECTING;
					LOG_C("main: UNEXPECTED EVENT (bits: 0x%04x)", bits);
				}
				break;

#if TARGET == TARGET_TX
      case STATE_WIFI_CONNECTED:
			if (!adc_inited)
			{
				adc_init();
				adc_inited = true;
			}
			ADC_Data_t adc_data;
			bool adc_data_valid = false;
			if (adc_read(&adc_data))
			{
				adc_counter++;
				if (adc_counter >= 20)
				{
					// LOG_D("ADC_DATA: %d, %d, %d, %d, %d, %d", adc_data.value[0], adc_data.value[1], adc_data.value[2], adc_data.value[3], adc_data.value[4], adc_data.value[5]);
					adc_data_valid = true;
					adc_counter = 0;
				}
			}

			for (WS_conn_proc = 0; WS_conn_proc <3; WS_conn_proc++)
			{
				switch (WSStates[WS_conn_proc])
				{
					case STATE_WS_CONNECTING:
						LOG_I("main: websocket %d: connecting", WS_conn_proc);
						ws_client[WS_conn_proc] = ws_connect(WS_URL, BoatId + WS_conn_proc, WS_DEV_CODE_CLIENT);
						WSStates[WS_conn_proc] = STATE_WS_WAIT_CONNECTION;
						break;
					case STATE_WS_WAIT_CONNECTION:
						if (ws_client[WS_conn_proc]->connected)
						{
							LOG_I("main: websocket %d: connected", WS_conn_proc);
							WSStates[WS_conn_proc] = STATE_IDEL;
						}
						break;

					case STATE_IDEL:
					   if (adc_data_valid)
						{
							char cmd[100];
							snprintf(&cmd[0], 100, "%04u;ctrl;%04u",
								ws_client[WS_conn_proc]->id,
								ws_client[WS_conn_proc]->id
							);
							ws_sendString(ws_client[WS_conn_proc], &cmd[0]);
						}
					case STATE_DRIVING:
						if (!ws_client[WS_conn_proc]->connected)
						{
							WSStates[WS_conn_proc] = STATE_WS_WAIT_CONNECTION;
						}

						char ws_resp[10];
						uint16_t ws_resp_len;
						while ((ws_resp_len = ws_getstr(ws_client[WS_conn_proc], 10, &ws_resp[0])) > 0)
						{
							LOG_D("main: websocket %d: resv: %.*s", WS_conn_proc, ws_resp_len, ws_resp);
							if (ws_resp_len == 4)
							{
								ws_resp[4] = 0;
								if (strcmp(&ws_resp[0], "FAIL") == 0)
								{
									WSStates[WS_conn_proc] = STATE_IDEL;
								}
							}
							else if (ws_resp_len == 2)
							{
								ws_resp[2] = 0;
								if (strcmp(&ws_resp[0], "OK") == 0)
								{
									WSStates[WS_conn_proc] = STATE_DRIVING;
								}
							}
						}

						if ((WSStates[WS_conn_proc] == STATE_DRIVING) && adc_data_valid)
						{
							char ctrl_cmd[100];
							snprintf(&ctrl_cmd[0], 100, "%04u;d;%u,%u",
								ws_client[WS_conn_proc]->id,
								adc_data.value[WS_conn_proc][0] >> 4,
								adc_data.value[WS_conn_proc][1] >> 4
							);
							ws_sendString(ws_client[WS_conn_proc], &ctrl_cmd[0]);
						}

						break;
				}
			}
			break;
#elif TARGET == TARGET_RX
			case STATE_WS_CONNECTING:
				ws_client = ws_connect(WS_URL, BoatId, WS_DEV_CODE_BOAT);
				MainState = STATE_WS_WAIT_CONNECTION;
				break;
			case STATE_WS_WAIT_CONNECTION:
				if (ws_client->connected)
				{
					cli_ws_client = CLI_init((CLI_charOutFn)&charOut_ws, cmdList);
					cli_ws_client.echo = false;
					MainState = STATE_IDEL;
				}
				break;

			case STATE_IDEL:
			case STATE_DRIVING:
				if (!ws_client->connected)
				{
					CLI_deinit(&cli_ws_client);
					MainState = STATE_WS_WAIT_CONNECTION;
				}

				while ((charIn = ws_getchar(ws_client)) != 255)
				{
					CLI_charIn(&cli_ws_client, charIn);
				}
				ws_sendData(ws_client);
				break;
#endif
		}
	}

#if TARGET == TARGET_TX
	led_setRGB(0, 0, 0);
#endif

	servo_deinit();
	CLI_deinit(&cli_uart);
	CMDList_deinit(cmdList);

	return;
}