#include "esp_task_wdt.h"
#include "esp_system.h"
#include "i2c.h"
#include "log_test.h"
#include "storage.h"
#include "uart_comms.h"
#include "esp_err.h"
#include "esp_log.h"
#include "endian.h"
#include "control_fsm.h"
#include "power_mgmt.h"
#include "rtc.h"
#include "sensors.h"
#include "solar.h"
#include "rf_433.h"
#include "bt_hid.h"
#include "webserver.h"
#include "version.h"
#include <string.h>

#define TAG "MAIN"

int64_t last_bat_log_time = 0;
esp_err_t send_bat_log() {
	if(!rtc_is_set()) return ESP_OK;
	
	uint8_t entry[12] = {};
	
    // Pack 64-bit timestamp into bytes 1-8
    uint64_t be_timestamp = rtc_get_ms();
    memcpy(&entry[0], &be_timestamp, 8);
    
    // Pack 32-bit voltages/currents into bytes 9-24
    float be_voltage = get_battery_V();
    memcpy(&entry[8],  &be_voltage,  4);
    
    last_bat_log_time = esp_timer_get_time();
    
    log_write(entry, 12, LOG_TYPE_BAT);
    
    return ESP_OK;
}


typedef enum {
	LED_STATE_DRIVING,
	LED_STATE_ERROR,
	LED_STATE_AWAKE,
	LED_STATE_CANCELLING,
	LED_STATE_ERRORED,
	LED_STATE_START1,
	LED_STATE_START2,
	LED_STATE_START3,
	LED_STATE_START4,
	LED_STATE_BOOTING
} led_state_t;

void drive_leds(led_state_t state) {
	uint8_t patterns[5][12] = {
		{1,3,7,6,4,0},
		{0b101,0b001},
		{1,1,1,1,1,1, 1,1,1,3},
		{4,2},
		{0b001, 0b101},
	};
	switch(state) {
		case LED_STATE_DRIVING:
			i2c_set_led1(patterns[state][(esp_timer_get_time()/100000) % 6]);
			break;
		case LED_STATE_ERROR:
			//ESP_LOGE(TAG, "SOME SORT OF ERROR");
			i2c_set_led1(patterns[state][(esp_timer_get_time()/1000000) % 2]);
			break;
		case LED_STATE_AWAKE:
			i2c_set_led1(patterns[state][(esp_timer_get_time()/200000) % 10]);
			break;
		case LED_STATE_CANCELLING:
			i2c_set_led1(patterns[state][(esp_timer_get_time()/200000) % 2]);
			break;
		
		case LED_STATE_ERRORED:
			i2c_set_led1(patterns[state][(esp_timer_get_time()/200000) % 2]);
			break;
			
		case LED_STATE_BOOTING:
			i2c_set_led1(0b001);
			break;
			
		case LED_STATE_START1:
			i2c_set_led1(0b000);
			break;
		case LED_STATE_START2:
			i2c_set_led1(0b001);
			break;
		case LED_STATE_START3:
			i2c_set_led1(0b011);
			break;
		case LED_STATE_START4:
			i2c_set_led1(0b111);
			break;
	}
}

void app_main(void) {esp_task_wdt_add(NULL);

	//run_all_log_tests();

    ESP_LOGI(TAG, "Firmware: %s", FIRMWARE_STRING);
    ESP_LOGI(TAG, "Version: %s", FIRMWARE_VERSION);
    ESP_LOGI(TAG, "Branch: %s", FIRMWARE_BRANCH);
    ESP_LOGI(TAG, "Built: %s", BUILD_DATE);
    
    // TODO: Check wdt stuff
    // TODO: Stack Overflow Detection
	// TODO: Remove XTAL crystal stuff
    if (rtc_xtal_init() != ESP_OK) ESP_LOGE(TAG, "RTC FAILED");
    rtc_restore_time();  // Recover time from RTC domain if we crashed

    // Say hello; turn on the lights
    rtc_wakeup_cause();  // log wakeup cause (informational only) // TODO: Shouldnt be needed anymore
    if (i2c_init()      != ESP_OK) ESP_LOGE(TAG, "I2C FAILED");
    i2c_set_relays((relay_port_t){.raw=0});
    drive_leds(LED_STATE_BOOTING);
    
    // TODO: How many tasks do we have?
    
    
    // Check for factory reset condition: Cold boot (power-on/ext-reset) + button held
    esp_reset_reason_t boot_reset_reason = esp_reset_reason();
    if ((boot_reset_reason == ESP_RST_POWERON || boot_reset_reason == ESP_RST_EXT)
        && gpio_get_level(GPIO_NUM_13) == 0) {
        ESP_LOGW(TAG, "FACTORY RESET TRIGGERED - Button held on cold boot");
        
        // Flash LED pattern to indicate factory reset
        for (int i = 0; i < 10; i++) {
            i2c_set_led1(0b111);
            vTaskDelay(pdMS_TO_TICKS(100));
            i2c_set_led1(0b000);
            vTaskDelay(pdMS_TO_TICKS(100));
        }
        
        // Initialize minimal components needed for factory reset
        if (storage_init()  != ESP_OK) ESP_LOGE(TAG, "STORAGE FAILED");
        
        // Perform factory reset
        esp_err_t reset_err = factory_reset();
        if (reset_err == ESP_OK) {
            ESP_LOGI(TAG, "Factory reset completed successfully");
            // Flash success pattern
            for (int i = 0; i < 5; i++) {
                i2c_set_led1(0b010);
                vTaskDelay(pdMS_TO_TICKS(200));
                i2c_set_led1(0b000);
                vTaskDelay(pdMS_TO_TICKS(200));
            }
        } else {
            ESP_LOGE(TAG, "Factory reset failed!");
            // Flash error pattern
            for (int i = 0; i < 5; i++) {
                i2c_set_led1(0b100);
                vTaskDelay(pdMS_TO_TICKS(200));
                i2c_set_led1(0b000);
                vTaskDelay(pdMS_TO_TICKS(200));
            }
        }
        
        // Reboot the system
        ESP_LOGI(TAG, "Rebooting system...");
        vTaskDelay(pdMS_TO_TICKS(1000));
        esp_restart();
    }
    
    // Every boot we load parameters and monitor solar, no matter what
	// TODO: Do things with errors (put in real log? then reset. "assert with LOGE"?)
	if (adc_init()      != ESP_OK) ESP_LOGE(TAG, "ADC FAILED");
    if (storage_init()  != ESP_OK) ESP_LOGE(TAG, "STORAGE FAILED");
    if (log_init()      != ESP_OK) ESP_LOGE(TAG, "LOG FAILED");
    // TODO: figure out how long logging takes (for reference, and comp to wdt)
    
    esp_reset_reason_t reset_reason = esp_reset_reason();
    esp_sleep_wakeup_cause_t wake_cause = esp_sleep_get_wakeup_cause();

    // Log every boot: boot_info = wake_cause[7:4] | reset_reason[3:0]
    {
        uint8_t boot_entry[9] = {};
        uint64_t ts = rtc_get_ms();
        memcpy(&boot_entry[0], &ts, 8);
        boot_entry[8] = ((uint8_t)wake_cause << 4) | ((uint8_t)reset_reason & 0x0F);
        log_write(boot_entry, sizeof(boot_entry), LOG_TYPE_BOOT);
    }

	// TODO: make sure that this is "crash proof"
	// TODO: OTA rollback (triggered how? preferably with hardware... or if there are 5 resets in a row [check bootloader?]. also need way to nuke the storage partition or safe boot)
	// TODO: (maybe) recovery partition that allows uploading firmware
    // Write a crash log entry if we rebooted unexpectedly
    if (reset_reason == ESP_RST_PANIC    ||
        reset_reason == ESP_RST_INT_WDT  ||
        reset_reason == ESP_RST_TASK_WDT ||
        reset_reason == ESP_RST_WDT) {
        ESP_LOGW(TAG, "Crash detected! Reset reason: %d", reset_reason);
        uint8_t crash_entry[9] = {};
        uint64_t ts = rtc_get_ms();
        memcpy(&crash_entry[0], &ts, 8);
        crash_entry[8] = (uint8_t)reset_reason;
        log_write(crash_entry, sizeof(crash_entry), LOG_TYPE_CRASH);
    }

	// TODO: is this reasonable now that we eliminated deep sleep?
    if (solar_run_fsm() != ESP_OK) ESP_LOGE(TAG, "SOLAR FAILED");
    // TODO: Do a 12V check and enter deep sleep if there's a problem

    send_bat_log();
    
    // TODO: test strategy!!! (software verification, and unit bringup)
    // TODO: A->D bringup; sanity check (sum up all inputs, wait 5ms, sum again, make sure there is a change (not frozen))
    
    // TODO: make sure sdkconfig is sane. Make notes, have claude figure this out properly
    // TODO: fix managed_components
    
	//send_log();
	
	//write_dummy_log_1();
    
    /*** FULL BOOT — always, every boot ***/
    if (uart_init()    != ESP_OK) ESP_LOGE(TAG, "UART FAILED");
    //if (power_init()   != ESP_OK) ESP_LOGE(TAG, "POWER FAILED");
    
    // TODO: Seriously, log all the errors on bluetooth
    if (rf_433_init()  != ESP_OK) ESP_LOGE(TAG, "RF FAILED");
    if (bt_hid_init()  != ESP_OK) ESP_LOGE(TAG, "BT HID FAILED");
    if (fsm_init()     != ESP_OK) ESP_LOGE(TAG, "FSM FAILED");
    //if (sensors_init() != ESP_OK) ESP_LOGE(TAG, "SENSORS FAILED"); // TODO: Why is this off?
    if (webserver_init() != ESP_OK) ESP_LOGE(TAG, "WEBSERVER FAILED");

    /*** MAIN LOOP ***/
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(50);

    while(true) {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
        
        /* In soft idle: slow poll (5s) via direct GPIO, no I2C. */
        // TODO: Critique & confirm what we do in idle
        if (soft_idle_is_active()) {
            //vTaskDelay(pdMS_TO_TICKS(1000));
            if (soft_idle_button_raw()) {
                rtc_reset_shutdown_timer();
                soft_idle_exit();
                i2c_poll_buttons();  /* sync TCA9555 state after idle */
                xLastWakeTime = xTaskGetTickCount();
            }
            if (rtc_alarm_tripped()) {
                soft_idle_exit();
                xLastWakeTime = xTaskGetTickCount();
                vTaskDelay(pdMS_TO_TICKS(500));
                // TODO: do a hard wait until wifi and bluetooth come up, not just blindly wait; might be better to be non-blocking
                fsm_request(FSM_CMD_START);
                rtc_schedule_next_alarm();
            }
            solar_run_fsm();
            rtc_check_shutdown_timer();
            esp_task_wdt_reset();
            continue;
        }

        i2c_poll_buttons();

        if (i2c_get_button_state(0)) {
        	rtc_reset_shutdown_timer();
        	soft_idle_exit();
        }
        
        // TODO: Make sure all ISRs are clean (very tight, no blocking functions)
        	
        switch (fsm_get_state()) {
			case STATE_IDLE:
        		// LED cue for user
        		if (i2c_get_button_ms(0) > 1600){
        			drive_leds(LED_STATE_START4);
				} else if (i2c_get_button_ms(0) > 1100){
        			drive_leds(LED_STATE_START3);
				} else if (i2c_get_button_ms(0) > 600){
        			drive_leds(LED_STATE_START2);
        		} else if (i2c_get_button_ms(0) > 100){
        			drive_leds(LED_STATE_START1);
        		} else {
					if (
						rtc_is_set() &&
						efuse_get(BRIDGE_JACK)==EFUSE_OK &&
						efuse_get(BRIDGE_AUX)==EFUSE_OK &&
						efuse_get(BRIDGE_DRIVE)==EFUSE_OK &&
						fsm_get_error() == ESP_OK
					) {
        				drive_leds(LED_STATE_AWAKE);
        			} else {
        				drive_leds(LED_STATE_ERROR);
        			}
        			
        			/*int8_t state = 0b001;
        			if (get_is_safe()) state |= 0b010;
        			if (get_sensor(SENSOR_SAFETY)) state |= 0b100;
        			i2c_set_led1(state);*/
				}
				
				// when not actively moving we log at a low frequency (every 120s)
				if ((esp_timer_get_time() > last_bat_log_time + 120000000ULL))
				  send_bat_log();
				
				if(i2c_get_button_ms(0) > 2100)
        			fsm_request(FSM_CMD_START);
        		break;
			//case STATE_UNDO_JACK:
			case STATE_UNDO_JACK_START:
				// it's running the jack, but undoing
				//send_log();
        		drive_leds(LED_STATE_CANCELLING);
				if (i2c_get_button_tripped(0)) {
					ESP_LOGI(TAG, "AAAAH STOP!!!");
        			fsm_request(FSM_CMD_STOP);
				}
        		break;
        		
        	case STATE_CALIBRATE_JACK_DELAY:
        		//send_log();
        		if (i2c_get_button_tripped(0))
	        		fsm_request(FSM_CMD_CALIBRATE_JACK_START);
        		break;
        	case STATE_CALIBRATE_JACK_MOVE:
        		//send_log();
        		if (i2c_get_button_tripped(0))
	        		fsm_request(FSM_CMD_CALIBRATE_JACK_END);
        		break;
        		
        		
        	case STATE_CALIBRATE_DRIVE_DELAY:
        		//send_log();
        		if (i2c_get_button_tripped(0))
	        		fsm_request(FSM_CMD_CALIBRATE_DRIVE_START);
        		break;
        	case STATE_CALIBRATE_DRIVE_MOVE:
        		//send_log();
        		if (i2c_get_button_tripped(0))
	        		fsm_request(FSM_CMD_CALIBRATE_DRIVE_END);
        		break;
        		
			default:
				// it's running in every other case
				//send_log();
        		drive_leds(LED_STATE_DRIVING);
				if (i2c_get_button_tripped(0)) {
        			fsm_request(FSM_CMD_UNDO);
				}
        		break;
		}
        
        
        
        
        if (rtc_alarm_tripped()) {
        	fsm_request(FSM_CMD_START);
        	rtc_schedule_next_alarm();
        }

        solar_run_fsm();
        rtc_check_shutdown_timer(); // TODO: Will esp timer overflow? Handle overflow if needed (this used to be handled by the fact that we were in deep sleep)
        esp_task_wdt_reset();
	}
}