// File: main.c
#include "App_Common.h"

// --- Global Variables ---
uint32_t AppBuff[APPBUFF_SIZE];
AppMode CurrentMode = MODE_IDLE;
float LFOSCFreq = 32000.0;
uint32_t g_AmpIndex = 0;
uint32_t g_RampIndex = 0;

uint32_t ConfigLptiaVal = 100000;
uint32_t ConfigHstiaVal = 1000;
uint32_t CurrentLpTiaRf = LPTIARF_20K;
uint32_t ConfigRLoad = LPTIARLOAD_100R;
float CalibratedLptiaVal = 100000.0;
float CalibratedHstiaVal = 1000.0; // Default nominal
float CalibratedHstiaPhase = 0.0;  // Default 0 phase shift
BoolFlag GlobalShortRe0Se0 = bFALSE;

char input_buffer[64];
int input_pos = 0;

void process_command() {
  char cmd = input_buffer[0];
  sleep_ms(10);

  if (cmd == 'v') {
    uint32_t id = AD5940_ReadReg(REG_AFECON_CHIPID);
    printf("CHIP_ID:0x%04X\n", id);
  } else if (cmd == 'r') {
    if (strlen(input_buffer) > 2) {
      int hp = 0;
      int count = sscanf(input_buffer + 2, "%d", &hp);
      if (count >= 1) {
        ConfigHstiaVal = hp;
        CalibratedHstiaVal =
            (float)hp; // Fix: Reset calibration to nominal when range changes
      }
      printf("RANGE_SET HP:%d\n", ConfigHstiaVal);
    }
  } else if (cmd == 't') {
    if (strlen(input_buffer) > 2) {
      int val = atoi(input_buffer + 2);
      GlobalShortRe0Se0 = (val > 0) ? bTRUE : bFALSE;
      printf("SHORT_RE0_SE0:%d\n", GlobalShortRe0Se0);
    }
  } else if (cmd == 'c') {
    Routine_CalibrateLFO();
    Routine_CalibrateSystem();
  } else if (cmd == 'm') {
    float freq = 1000.0f;
    float bias = 0.0f;
    if (strlen(input_buffer) > 2) {
      int count = sscanf(input_buffer + 2, "%f %f", &freq, &bias);
      // If only one arg provided, freq is set, bias remains 0.
    }
    // Routine_Measure needs to handle bias now.
    // We'll update Routine_Measure momentarily.
    // For now, let's just pass freq as before, but update Routine_Measure
    // signature.
    Routine_Measure(freq, bias);
  } else if (cmd == 's') {
    float start = 100.0f, end = 100000.0f;
    int steps = 50;
    float bias = 0.0f;
    if (strlen(input_buffer) > 2) {
      int count =
          sscanf(input_buffer + 2, "%f %f %d %f", &start, &end, &steps, &bias);
      // defaults handled by initialization
    }
    Routine_Sweep(start, end, steps, bias);
  } else if (cmd == 'a') {
    float bias = 0.0f;
    if (strlen(input_buffer) > 2)
      bias = atof(input_buffer + 2);
    Routine_Amperometric(bias);
  } else if (cmd == 'l') {
    float start = -500.0f, end = 500.0f;
    int steps = 100, duration = 10000;
    if (strlen(input_buffer) > 2)
      sscanf(input_buffer + 2, "%f %f %d %d", &start, &end, &steps, &duration);
    Routine_LSV(start, end, steps, duration);
  } else if (cmd == 'x') {
    if (CurrentMode == MODE_IMPEDANCE)
      AppIMPCleanup();
    else if (CurrentMode == MODE_AMPEROMETRIC)
      AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
    else if (CurrentMode == MODE_RAMP)
      AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
    CurrentMode = MODE_IDLE;
    printf("STOPPED\n");
    SystemReset();
  } else if (cmd == 'z') {
    SystemReset();
  }
}

int main() {
  stdio_init_all();
  sleep_ms(2000);

  setup_pins();

  AD5940_HWReset();
  AD5940_Initialize();
  AD5940PlatformCfg();

  Routine_CalibrateLFO();

  printf("SYSTEM_READY\n");

  while (true) {
    int c = getchar_timeout_us(0);
    if (c != PICO_ERROR_TIMEOUT) {
      if (c == '\n' || c == '\r') {
        input_buffer[input_pos] = 0;
        if (input_pos > 0)
          process_command();
        input_pos = 0;
      } else if (input_pos < 63) {
        input_buffer[input_pos++] = (char)c;
      }
    }

    if (gpio_get(PIN_INT) == 0) {
      uint32_t temp = APPBUFF_SIZE;
      int32_t status = 0;

      if (CurrentMode == MODE_IMPEDANCE) {
        status = AppIMPISR(AppBuff, &temp);
        if (status == AD5940ERR_BUFF) {
          printf("ERROR: FIFO Overflow/Underflow. Stopping.\n");
          AppIMPCleanup();
          CurrentMode = MODE_IDLE;
          SystemReset();
        } else if (temp > 0) {
          ImpedanceShowResult(AppBuff, temp);
        }
      } else if (CurrentMode == MODE_AMPEROMETRIC) {
        status = AppAMPISR(AppBuff, &temp);
        if (status == AD5940ERR_BUFF) {
          printf("ERROR: FIFO Overflow/Underflow. Stopping.\n");
          AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
          CurrentMode = MODE_IDLE;
          SystemReset();
        } else if (temp > 0) {
          AmperometricShowResult((float *)AppBuff, temp);
        }
      } else if (CurrentMode == MODE_RAMP) {
        status = AppRAMPISR(AppBuff, &temp);
        if (status == AD5940ERR_BUFF) {
          printf("ERROR: FIFO Overflow/Underflow. Stopping.\n");
          AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
          CurrentMode = MODE_IDLE;
          SystemReset();
        } else if (temp > 0) {
          RampShowResult((float *)AppBuff, temp);
        }
      }

      if (status == AD5940ERR_STOP) {
        printf("STOPPED\n");
        if (CurrentMode == MODE_IMPEDANCE)
          AppIMPCleanup();
        else if (CurrentMode == MODE_AMPEROMETRIC)
          AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
        else if (CurrentMode == MODE_RAMP)
          AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
        CurrentMode = MODE_IDLE;
        SystemReset();
      }
    }
  }
  return 0;
}