// File: main.c
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "hardware/spi.h"
#include "hardware/gpio.h"
#include "hardware/watchdog.h"
#include "ad5940.h"
#include "Impedance.h"

// ---------------------------------------------------------------------------
// Hardware Definitions
// ---------------------------------------------------------------------------
#define PIN_MISO 0
#define PIN_CS   1
#define PIN_SCK  2
#define PIN_MOSI 3
#define PIN_RST  9
#define PIN_INT  29 

#define APPBUFF_SIZE 512
uint32_t AppBuff[APPBUFF_SIZE];

// ---------------------------------------------------------------------------
// Platform Interface Implementation
// ---------------------------------------------------------------------------
void AD5940_CsClr(void) { gpio_put(PIN_CS, 0); }
void AD5940_CsSet(void) { gpio_put(PIN_CS, 1); }
void AD5940_RstClr(void) { gpio_put(PIN_RST, 0); }
void AD5940_RstSet(void) { gpio_put(PIN_RST, 1); }
void AD5940_Delay10us(uint32_t time) { sleep_us(time * 10); }
void AD5940_ReadWriteNBytes(unsigned char *pSendBuffer, unsigned char *pRecvBuff, unsigned long length) {
    spi_write_read_blocking(spi0, pSendBuffer, pRecvBuff, length);
}

uint32_t AD5940_GetMCUIntFlag(void) {
    return (gpio_get(PIN_INT) == 0);
}

uint32_t AD5940_ClrMCUIntFlag(void) {
    return 1;
}

uint32_t AD5940_MCUResourceInit(void *pCfg) {
    return 0;
}

void AD5940_MCUGpioWrite(uint32_t data) { (void)data; }
uint32_t AD5940_MCUGpioRead(uint32_t pin) { (void)pin; return 0; }
void AD5940_MCUGpioCtrl(uint32_t pin, BoolFlag enable) { (void)pin; (void)enable; }

// ---------------------------------------------------------------------------
// Application Logic
// ---------------------------------------------------------------------------

void setup_pins(void) {
    // Increase SPI speed to 4MHz to prevent FIFO overflow during fast sweeps
    spi_init(spi0, 4000000); 
    gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
    gpio_set_function(PIN_SCK, GPIO_FUNC_SPI);
    gpio_set_function(PIN_MOSI, GPIO_FUNC_SPI);

    gpio_init(PIN_CS);
    gpio_set_dir(PIN_CS, GPIO_OUT);
    gpio_put(PIN_CS, 1);

    gpio_init(PIN_RST);
    gpio_set_dir(PIN_RST, GPIO_OUT);
    gpio_put(PIN_RST, 1);

    gpio_init(PIN_INT);
    gpio_set_dir(PIN_INT, GPIO_IN);
    gpio_pull_up(PIN_INT); 
}

void AD5940ImpedanceStructInit(void)
{
  AppIMPCfg_Type *pImpedanceCfg;
  
  AppIMPGetCfg(&pImpedanceCfg);
  
  pImpedanceCfg->SeqStartAddr = 0;
  pImpedanceCfg->MaxSeqLen = 512; 

  pImpedanceCfg->RcalVal = 100.0; 
  pImpedanceCfg->RtiaVal = 200.0; 
  pImpedanceCfg->SinFreq = 1000.0;
  pImpedanceCfg->FifoThresh = 6; 
	
  pImpedanceCfg->DacVoltPP = 600.0;
  pImpedanceCfg->ExcitBufGain = EXCITBUFGAIN_0P25;
  pImpedanceCfg->HsDacGain = HSDACGAIN_0P2;
	
  pImpedanceCfg->DswitchSel = SWD_CE0;
  pImpedanceCfg->PswitchSel = SWP_CE0;
  pImpedanceCfg->NswitchSel = SWN_SE0;
  pImpedanceCfg->TswitchSel = SWT_SE0LOAD; 

  pImpedanceCfg->HstiaRtiaSel = HSTIARTIA_200;	
  pImpedanceCfg->BiasVolt = 0.0;

  pImpedanceCfg->SweepCfg.SweepEn = bFALSE;
  pImpedanceCfg->SweepCfg.SweepStart = 100.0f;
  pImpedanceCfg->SweepCfg.SweepStop = 100000.0f;
  pImpedanceCfg->SweepCfg.SweepPoints = 50;
  pImpedanceCfg->SweepCfg.SweepLog = bTRUE;

  pImpedanceCfg->PwrMod = AFEPWR_LP;
  pImpedanceCfg->ADCSinc3Osr = ADCSINC3OSR_4;
  pImpedanceCfg->DftNum = DFTNUM_16384;
  pImpedanceCfg->DftSrc = DFTSRC_SINC3;
}

static int32_t AD5940PlatformCfg(void)
{
  CLKCfg_Type clk_cfg;
  FIFOCfg_Type fifo_cfg;
  AGPIOCfg_Type gpio_cfg;

  AD5940_HWReset();
  AD5940_Initialize();
  
  clk_cfg.ADCClkDiv = ADCCLKDIV_1;
  clk_cfg.ADCCLkSrc = ADCCLKSRC_HFOSC;
  clk_cfg.SysClkDiv = SYSCLKDIV_1;
  clk_cfg.SysClkSrc = SYSCLKSRC_HFOSC;
  clk_cfg.HfOSC32MHzMode = bFALSE;
  clk_cfg.HFOSCEn = bTRUE;
  clk_cfg.HFXTALEn = bFALSE;
  clk_cfg.LFOSCEn = bTRUE;
  AD5940_CLKCfg(&clk_cfg);
  
  fifo_cfg.FIFOEn = bFALSE;
  fifo_cfg.FIFOMode = FIFOMODE_FIFO;
  fifo_cfg.FIFOSize = FIFOSIZE_4KB;
  fifo_cfg.FIFOSrc = FIFOSRC_DFT;
  fifo_cfg.FIFOThresh = 6;		
  AD5940_FIFOCfg(&fifo_cfg);
  fifo_cfg.FIFOEn = bTRUE;
  AD5940_FIFOCfg(&fifo_cfg);
  
  AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_ALLINT, bTRUE); 
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
  AD5940_INTCCfg(AFEINTC_0, AFEINTSRC_DATAFIFOTHRESH, bTRUE); 
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  gpio_cfg.FuncSet = GP0_INT;
  gpio_cfg.InputEnSet = 0;
  gpio_cfg.OutputEnSet = AGPIO_Pin0;
  gpio_cfg.OutVal = 0;
  gpio_cfg.PullEnSet = 0;
  AD5940_AGPIOCfg(&gpio_cfg);
  
  AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK); 
  return 0;
}

void ImpedanceShowResult(uint32_t *pData, uint32_t DataCount)
{
  float freq;
  fImpPol_Type *pImp = (fImpPol_Type*)pData;
  AppIMPCtrl(IMPCTRL_GETFREQ, &freq);

  for(int i=0;i<DataCount;i++)
  {
    // Convert Polar (Mag, Phase) back to Cartesian (Real, Imag) for display
    float mag = pImp[i].Magnitude;
    float phase = pImp[i].Phase;
    
    float real = mag * cosf(phase);
    float imag = mag * sinf(phase);

    printf("DATA,%.2f,%.4f,%.4f,%.4f,%.4f\n", 
           freq, 
           mag, 
           phase * 180.0f / MATH_PI,
           real, 
           imag      
           );
  }
}

// ---------------------------------------------------------------------------
// Main Loop
// ---------------------------------------------------------------------------

char input_buffer[64];
int input_pos = 0;

// Helper for robust state cleanup
void AD5940_Main_Cleanup(void) {
    // Ensure chip is awake before sending commands
    AD5940_WakeUp(10);

    // 1. Stop Sequencer and Wakeup Timer
    AD5940_WUPTCtrl(bFALSE);
    AD5940_SEQCtrlS(bFALSE);
    
    // 2. Stop any active conversions/WG, but KEEP Reference/LDOs ON
    // This prevents settling issues on restart
    AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT|AFECTRL_WG, bFALSE);

    // 3. Reset FIFO
    FIFOCfg_Type fifo_cfg;
    fifo_cfg.FIFOEn = bFALSE;           
    AD5940_FIFOCfg(&fifo_cfg);          
    
    fifo_cfg.FIFOEn = bTRUE;            
    fifo_cfg.FIFOMode = FIFOMODE_FIFO;
    fifo_cfg.FIFOSize = FIFOSIZE_4KB;   
    fifo_cfg.FIFOSrc = FIFOSRC_DFT;     
    fifo_cfg.FIFOThresh = 6;            
    AD5940_FIFOCfg(&fifo_cfg);
    
    // 4. Clear Interrupts
    AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
}

void process_command() {
    char cmd = input_buffer[0];
    AppIMPCfg_Type *pCfg;
    AppIMPGetCfg(&pCfg);

    // CRITICAL: Perform robust cleanup before any new operation
    AD5940_Main_Cleanup();
    sleep_ms(10); // Give AFE time to settle

    if (cmd == 'v') {
        uint32_t id = AD5940_ReadReg(REG_AFECON_CHIPID);
        printf("CHIP_ID:0x%04X\n", id);
    } 
    else if (cmd == 'c') {
        // Cleanup already done by AD5940_Main_Cleanup() above

        // 5. Perform ADC Calibration (Offset)
        ADCPGACal_Type adcpga_cal;
        adcpga_cal.AdcClkFreq = 16000000.0;
        adcpga_cal.SysClkFreq = 16000000.0;
        adcpga_cal.ADCSinc3Osr = ADCSINC3OSR_4;
        adcpga_cal.ADCSinc2Osr = ADCSINC2OSR_22;
        adcpga_cal.ADCPga = ADCPGA_1P5; 
        adcpga_cal.PGACalType = PGACALTYPE_OFFSET;
        adcpga_cal.TimeOut10us = 1000;
        adcpga_cal.VRef1p11 = 1.11;
        adcpga_cal.VRef1p82 = 1.82;
        printf(">> Calibrating ADC Offset...\n");
        AD5940_ADCPGACal(&adcpga_cal);

        // 6. Configure HSDAC to Low Gain explicitly
        HSDACCfg_Type hsdac_cfg;
        hsdac_cfg.ExcitBufGain = EXCITBUFGAIN_0P25;
        hsdac_cfg.HsDacGain = HSDACGAIN_0P2;
        hsdac_cfg.HsDacUpdateRate = 7;
        AD5940_HSDacCfgS(&hsdac_cfg);

        // 7. Run RTIA Calibration
        HSRTIACal_Type hsrtia_cal;
        fImpPol_Type Res;
        
        memset(&hsrtia_cal, 0, sizeof(hsrtia_cal));

        hsrtia_cal.fFreq = 1000.0f;
        hsrtia_cal.AdcClkFreq = 16000000.0;
        hsrtia_cal.SysClkFreq = 16000000.0;
        hsrtia_cal.ADCSinc3Osr = ADCSINC3OSR_4;
        hsrtia_cal.ADCSinc2Osr = ADCSINC2OSR_22;
        hsrtia_cal.bPolarResult = bTRUE; 
        hsrtia_cal.fRcal = 100.0;           
        hsrtia_cal.HsTiaCfg.DiodeClose = bFALSE;
        hsrtia_cal.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
        hsrtia_cal.HsTiaCfg.HstiaCtia = 31;
        hsrtia_cal.HsTiaCfg.HstiaRtiaSel = HSTIARTIA_200; 
        
        // CRITICAL FIX: Explicitly OPEN the DE0 switches. 
        // memset set them to 0 (50 Ohm / 0 Ohm), causing the 475 Ohm error.
        hsrtia_cal.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
        hsrtia_cal.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;

        hsrtia_cal.DftCfg.DftNum = DFTNUM_16384;
        hsrtia_cal.DftCfg.DftSrc = DFTSRC_SINC3;

        printf(">> Calibrating HSTIARTIA_200...\n");
        if (AD5940_HSRtiaCal(&hsrtia_cal, &Res) == AD5940ERR_OK) {
            printf("Calibrated Rtia: Mag = %f Ohm, Phase = %f\n", Res.Magnitude, Res.Phase);
            // Update global config with actual calibrated value to fix scaling error
            pCfg->RtiaVal = Res.Magnitude; 
        } else {
            printf("Calibration Failed\n");
        }
    }
    else if (cmd == 'm') {
        // Measure: Continuous monitoring at fixed frequency
        float freq = 1000.0f;
        if (strlen(input_buffer) > 2) freq = atof(input_buffer + 2);
        
        pCfg->SweepCfg.SweepEn = bFALSE;
        pCfg->SinFreq = freq;
        pCfg->NumOfData = -1; // Continuous
        
        // CRITICAL FIX: Force parameter change flag to TRUE.
        // This ensures AppIMPInit calls AppIMPSeqCfgGen, which re-enables
        // the analog blocks (DAC, TIA, etc.) that AD5940_Main_Cleanup turned off.
        pCfg->bParaChanged = bTRUE; 
        
        if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
            AppIMPCtrl(IMPCTRL_START, 0);
        } else {
            printf("ERROR: Init Failed\n");
        }
    }
    else if (cmd == 's') {
        // Sweep: Run exactly 'steps' measurements then stop
        float start = 100.0f, end = 100000.0f;
        int steps = 50;
        if (strlen(input_buffer) > 2) sscanf(input_buffer + 2, "%f %f %d", &start, &end, &steps);
        
        pCfg->SweepCfg.SweepEn = bTRUE;
        pCfg->SweepCfg.SweepStart = start;
        pCfg->SweepCfg.SweepStop = end;
        pCfg->SweepCfg.SweepPoints = steps;
        pCfg->SweepCfg.SweepLog = bTRUE;
        pCfg->NumOfData = steps; // Stop after sweep
        
        // CRITICAL FIX: Force parameter change flag to TRUE.
        pCfg->bParaChanged = bTRUE; 
        
        if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
            AppIMPCtrl(IMPCTRL_START, 0);
        } else {
            printf("ERROR: Init Failed\n");
        }
    }
    else if (cmd == 'x') {
        // Stop Measurement
        // Cleanup already done by AD5940_Main_Cleanup() at start of function
        printf("STOPPED\n");
    }
    else if (cmd == 'z') {
        // Full System Reset
        watchdog_reboot(0, 0, 0);
    }
}

int main() {
    stdio_init_all();
    sleep_ms(2000);
    
    setup_pins();
    AD5940PlatformCfg();
    AD5940ImpedanceStructInit();
    
    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;
            AppIMPISR(AppBuff, &temp);
            if(temp > 0) {
                ImpedanceShowResult(AppBuff, temp);
            }
        }
    }
    return 0;
}