#include "driver/gpio.h"
#include "driver/ledc.h"
#include "driver/usb_serial_jtag.h"
#include "esp_err.h"
#include "esp_vfs_dev.h"
#include "esp_vfs_usb_serial_jtag.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.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 512
#define PHASE_180 512

typedef enum {
  ST_DISABLED,
  STATE_ALL_ON,
  STATE_ALL_OFF,
  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_ALL_ON:
    gpio_set_level(PIN_A, 1);
    gpio_set_level(PIN_B, 1);
    break;

  case STATE_ALL_OFF:
    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, "ALL_ON") == 0){
        current_state = STATE_ALL_ON;
        apply_hardware();
        printf("ACK ALL_ON\n");
      } else if (strcmp(line, "ALL_OFF") == 0){
        current_state = STATE_ALL_OFF;
        apply_hardware();
        printf("ACK ALL_OFF\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));
  }
}
