// ARDUINO DUE: ROBUSTNI DMA RADAR (ODSTRANITEV DC + PRILAGOJEN ADC)
// Strojna oprema: Signal na pinu A1 (ADC kanal 6)

#include "arduinoFFT.h"

// --- KONFIGURACIJA ---
#define SAMPLES 1024             // Število vzorcev
#define SAMPLING_FREQUENCY 2048  // Frekvenca vzorčenja v Hz


// --- TABELA ---
const uint16_t FM_FREQ=2;
const uint16_t FM_SAMPLES=4096; // 12 bitov
uint16_t FM_table_pos[FM_SAMPLES];
uint16_t FM_table_neg[FM_SAMPLES];

int DAC_working=0;
// --- MEDPOMNILNIKI (BUFFERS) ---
volatile uint16_t bufferA[SAMPLES];
volatile uint16_t bufferB[SAMPLES];
volatile bool bufferA_Ready = false; 
volatile bool bufferB_Ready = false;

float vReal[SAMPLES];
float vImag[SAMPLES];

ArduinoFFT<float> FFT = ArduinoFFT<float>(vReal, vImag, SAMPLES, SAMPLING_FREQUENCY);

void setup() {
  // 1. Postavimo serijsko komunikacijo
  SerialUSB.begin(115200);
  while (!SerialUSB); // Počakaj na povezavo z računalnikom
  delay(5000);

  // 2. Poračunamo wavetable za FM signal
  for (uint16_t i=0; i<FM_SAMPLES; i++) {
    FM_table_pos[i] = i;      // Napolnimo tabelo z linearnim sweepom navzgor
    FM_table_neg[i] = 4095-i; // Napolnimo tabelo z linearnim sweepom navzdol
  }

  // 3. DACC - Digitalno-Analogni pretvornik
  // Krmiljenje VCO Tune signala

  pmc_enable_periph_clk(ID_DACC); // Vklopi clock za DACC
  
  DACC->DACC_CR = DACC_CR_SWRST; // Resetiraj DACC


  DACC->DACC_MR = DACC_MR_TRGSEL(2)| // Proženje na Timer Channel 1
                  DACC_MR_WORD_HALF| // Prenos po 16-bitov naenkrat
                  DACC_MR_STARTUP_512| // 512-bitni startup cikel
                  DACC_MR_USER_SEL_CHANNEL0| // Izhod na DAC0
                  DACC_MR_REFRESH(0)| // Brez refresha, ker se signal spremenja hitreje od Sample&Hold trajanja
                  DACC_MR_TAG_DIS| // brez TAGanja, ker vedno pišemo DAC0
                  DACC_MR_MAXS_NORMAL; // Normalni, ne Max Speed mode

  DACC->DACC_CHER = DACC_CHER_CH0; // Enablaj DAC0 izhod
  DACC->DACC_PTCR = DACC_PTCR_TXTDIS; // Disablaj DACC DMA kontroler
  DACC->DACC_TPR = (uint32_t) FM_table_pos; // Najprej začni z pozitivnim sweepom
  DACC->DACC_TNPR = (uint32_t) FM_table_neg; // Zatem z negativnim
  DACC->DACC_TCR = FM_SAMPLES; // Dolžina sweep tabele
  DACC->DACC_TNCR = FM_SAMPLES;
  DACC->DACC_PTCR = DACC_PTCR_TXTEN; // Enablaj DACC DMA kontroler

  
  NVIC_EnableIRQ(DACC_IRQn); // Enablaj prekinitev za DACC
  DACC->DACC_IDR = DACC_IDR_TXRDY | DACC_IDR_EOC;   // Izklopi TXRDY, EOC prekinitve
  DACC->DACC_IER = DACC_IER_ENDTX;                  // Vklopi ENDTX prekinitev (ko DAC preleti eno tabelo)
  
  DACC->DACC_MR |= DACC_MR_TRGEN_EN; // Vklopi proženje DACC
  DACC->DACC_CDR = FM_table_pos[0];  // Naloži prvo vrednost v DACC

  // 4. ADC - Analogno-Digitalni pretvornik
  // Vzorčenje povratnega signala v osnovnem pasu
  pmc_enable_periph_clk(ID_ADC);
  adc_init(ADC, SystemCoreClock, ADC_FREQ_MAX, ADC_STARTUP_FAST);

  // TRACKTIM(15): Poveča čas vzorčenja in zadrževanja (daljše čakanje, da se napetost ustali)
  // TRANSFER(3):  Optimizira čas prenosa med analognim in digitalnim delom
  // TRGEN / TRGSEL: Še vedno uporablja proženje s Timerjem 0
  ADC->ADC_MR = ADC_MR_TRGEN | 
                ADC_MR_TRGSEL_ADC_TRIG1 | 
                ADC_MR_TRACKTIM(15) | 
                ADC_MR_TRANSFER(3);

  ADC->ADC_CHER = 0x40; // Omogoči kanal 6 (Pin A1)

  // ADC DMA (PDC)
  ADC->ADC_PTCR = ADC_PTCR_RXTDIS;   // 
  ADC->ADC_RPR = (uint32_t)bufferA;  // Kazalec na trenutni medpomnilnik
  ADC->ADC_RCR = SAMPLES;            // Število vzorcev za prejem
  ADC->ADC_RNPR = (uint32_t)bufferB; // Kazalec na naslednji medpomnilnik
  ADC->ADC_RNCR = SAMPLES;           // Število vzorcev za naslednji prejem
  ADC->ADC_PTCR = ADC_PTCR_RXTEN;    // Omogoči DMA prejem

  // ADC DMA prekinitev (Interrupt)
  NVIC_EnableIRQ(ADC_IRQn);
  ADC->ADC_IER = ADC_IER_ENDRX;      // Prekinitev ob koncu prejema medpomnilnika

  // 5. Timer Counterja TC0 in TC1
  // TC0: 2048 Hz: krmili vzorčenje signala v osnovnem pasu
  // TC1: VAR  Hz: krmili frekvenčni prelet VCO Tune signala
  pmc_enable_periph_clk(ID_TC0); // Clock TC0
  pmc_enable_periph_clk(ID_TC1); // Clock TC1

  // Channel 0 (ADC trigger) - unchanged
  TC_Configure(TC0, 0, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | 
                      TC_CMR_TCCLKS_TIMER_CLOCK2 | 
                      TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
  const uint32_t rc_adc = 5127; // 2048 Hz
  TC_SetRA(TC0, 0, rc_adc / 2);
  TC_SetRC(TC0, 0, rc_adc);

  // Channel 1 (DAC trigger) - use CLOCK1
  TC_Configure(TC0, 1, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | 
                      TC_CMR_TCCLKS_TIMER_CLOCK1 | 
                      TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);  

  const uint32_t dac_period = 84000000/2/4096/FM_FREQ;
  TC_SetRA(TC0, 1, dac_period / 2);   
  TC_SetRC(TC0, 1, dac_period);

  // Sinhronizacija Timer bloka
  TC0->TC_BCR = TC_BCR_SYNC;

  // Zagon timerja
  TC_Start(TC0, 0);
  TC_Start(TC0, 1);

  pinMode(2, OUTPUT);
  digitalWrite(2, HIGH); // Vklop VCO-ja
}

void loop() {
  if (bufferA_Ready || bufferB_Ready) {
    // Izberi pripravljen medpomnilnik
    volatile uint16_t* inputBuffer = bufferA_Ready ? bufferA : bufferB;

    // --- KORAK 1: IZRAČUN DC ODMIKA (OFFSET) ---
    // Seštejemo vse vzorce, da najdemo povprečno napetost
    float dcBias = 0;
    for (int i = 0; i < SAMPLES; i++) {
      dcBias += inputBuffer[i];
    }
    dcBias = dcBias / SAMPLES;

    // --- KORAK 2: POLNJENJE FFT POLJ IN ODSTRANITEV DC ---
    // Odštevanje dcBias
    for (int i = 0; i < SAMPLES; i++) {
      vReal[i] = (float)inputBuffer[i] - dcBias;
      vImag[i] = 0.0;
    }

    // --- KORAK 3: IZRAČUN FFT ---
    FFT.windowing(FFT_WIN_TYP_HAMMING, FFT_FORWARD); // HAMMING-ovo okno
    FFT.compute(FFT_FORWARD);
    FFT.complexToMagnitude();

    // --- KORAK 4: POŠILJANJE PODATKOV ---
    // Pošljemo samo prvo polovico spektra (SAMPLES / 2)
    SerialUSB.write((uint8_t*)vReal, (SAMPLES / 2) * sizeof(float));
    // Ponastavi zastavico
    if (bufferA_Ready) bufferA_Ready = false;
    else bufferB_Ready = false;


  }
}

// Prekinitveni program (ISR) za ADC
void ADC_Handler() {
  if ((ADC->ADC_ISR & ADC_ISR_ENDRX) == ADC_ISR_ENDRX) {
    // Preveri, kateri medpomnilnik je ADC pravkar napolnil
    // Opomba: RPR kaže na TRENUTNEGA, kar pomeni, da je prejšnji končan
    if (ADC->ADC_RPR == (uint32_t)bufferB) {
      bufferA_Ready = true;               
      ADC->ADC_RNPR = (uint32_t)bufferA;  // Pripravi Buffer A kot naslednjega
      ADC->ADC_RNCR = SAMPLES;
    } else {
      bufferB_Ready = true;               
      ADC->ADC_RNPR = (uint32_t)bufferB;  // Pripravi Buffer B kot naslednjega
      ADC->ADC_RNCR = SAMPLES;
    }
  }
}

void DACC_Handler(void) {
  uint32_t status = DACC->DACC_ISR;  // Beri status
  if (status & DACC_ISR_ENDTX) {
    if(DACC->DACC_TPR == (uint32_t)FM_table_pos) { // Izberemo nasprotno smer preleta, ko pridemo do konca tabele
      DACC->DACC_TNPR = (uint32_t)FM_table_pos;
      DACC->DACC_TNCR = FM_SAMPLES;
    } else {
      DACC->DACC_TNPR = (uint32_t)FM_table_pos;
      DACC->DACC_TNCR = FM_SAMPLES;
    }    
    
  }
}