#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/ledc.h"
#include "esp_err.h"
#include "driver/usb_serial_jtag.h"
#include "esp_vfs_dev.h"
#include "esp_vfs_usb_serial_jtag.h"
#include "driver/gpio.h"

#define PIN_A 33
#define PIN_B 32
#define LEDC_MODE LEDC_LOW_SPEED_MODE
#define TIMER LEDC_TIMER_0
#define BIT_RES LEDC_TIMER_10_BIT
#define DUTY_50 460
#define PHASE_180 512

typedef enum {
    ST_DISABLED,
    STATE_MANUAL_ON,  // A=0, B=1
    STATE_MANUAL_OFF, // A=1, B=0
    ST_MODULATING
} system_state_t;

static system_state_t current_state = ST_DISABLED;
static uint32_t current_freq = 100;

void apply_hardware() {
    // Always stop LEDC before changing mode or GPIO state
    ledc_stop(LEDC_MODE, LEDC_CHANNEL_0, 0);
    ledc_stop(LEDC_MODE, LEDC_CHANNEL_1, 0);
    
    // Ensure GPIOs are in a clean state
    gpio_reset_pin(PIN_A);
    gpio_reset_pin(PIN_B);
    gpio_set_direction(PIN_A, GPIO_MODE_OUTPUT);
    gpio_set_direction(PIN_B, GPIO_MODE_OUTPUT);

    switch (current_state) {
        case ST_DISABLED:
            gpio_set_level(PIN_A, 0);
            gpio_set_level(PIN_B, 0);
            break;

        case STATE_MANUAL_ON: // A=0, B=1
            gpio_set_level(PIN_A, 0);
            gpio_set_level(PIN_B, 1);
            break;

        case STATE_MANUAL_OFF: // A=1, B=0
            gpio_set_level(PIN_A, 1);
            gpio_set_level(PIN_B, 0);
            break;

        case ST_MODULATING: {
            ledc_timer_config_t timer_conf = {
                .speed_mode = LEDC_MODE,
                .timer_num = TIMER,
                .duty_resolution = BIT_RES,
                .freq_hz = current_freq,
                .clk_cfg = LEDC_AUTO_CLK
            };
            ledc_timer_config(&timer_conf);

            ledc_channel_config_t chan_a = {
                .speed_mode = LEDC_MODE,
                .channel = LEDC_CHANNEL_0,
                .timer_sel = TIMER,
                .intr_type = LEDC_INTR_DISABLE,
                .gpio_num = PIN_A,
                .duty = DUTY_50,
                .hpoint = 0
            };
            ledc_channel_config(&chan_a);

            ledc_channel_config_t chan_b = {
                .speed_mode = LEDC_MODE,
                .channel = LEDC_CHANNEL_1,
                .timer_sel = TIMER,
                .intr_type = LEDC_INTR_DISABLE,
                .gpio_num = PIN_B,
                .duty = DUTY_50,
                .hpoint = PHASE_180
            };
            ledc_channel_config(&chan_b);
            break;
        }
    }
}

void app_main(void) {
    esp_vfs_dev_usb_serial_jtag_register();
    setvbuf(stdin, NULL, _IONBF, 0);
    setvbuf(stdout, NULL, _IONBF, 0);

    apply_hardware();

    printf("\nWAVEGEN_READY\n");

    char line[64];
    while (1) {
        if (fgets(line, sizeof(line), stdin)) {
            line[strcspn(line, "\r\n")] = 0;
            
            if (strcmp(line, "ENABLE") == 0) {
                current_state = STATE_MANUAL_OFF; // Default to OFF
                apply_hardware();
                printf("ACK ENABLED\n");
            } else if (strcmp(line, "DISABLE") == 0) {
                current_state = ST_DISABLED;
                apply_hardware();
                printf("ACK DISABLED\n");
            } else if (strcmp(line, "ON") == 0) {
                current_state = STATE_MANUAL_ON;
                apply_hardware();
                printf("ACK ON\n");
            } else if (strcmp(line, "OFF") == 0) {
                current_state = STATE_MANUAL_OFF;
                apply_hardware();
                printf("ACK OFF\n");
            } else if (strcmp(line, "MOD_ON") == 0) {
                current_state = ST_MODULATING;
                apply_hardware();
                printf("ACK MOD_ON\n");
            } else if (strcmp(line, "MOD_OFF") == 0) {
                current_state = STATE_MANUAL_OFF;
                apply_hardware();
                printf("ACK MOD_OFF\n");
            } else if (strncmp(line, "F ", 2) == 0) {
                uint32_t f = atoi(line + 2);
                if (f >= 10 && f <= 5000) {
                    current_freq = f;
                    if (current_state == ST_MODULATING) apply_hardware();
                    printf("ACK F %lu\n", f);
                } else printf("ERR_FREQ\n");
            } else if (strcmp(line, "PING") == 0) {
                printf("PONG\n");
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}