#include "Waveforms.h"

uint8_t  wf_wave0     = 0;
uint8_t  wf_pos       = 0;
uint16_t wf_freq      = 0;
uint8_t  wf_dutyCycle = 0;

unsigned long wf_sample_us  = 0;
unsigned long wf_prevMicros = 0;

bool wf_outputEnabled       = true;  // Variable to control waveform output state
bool wf_pwm_needs_disabling = true;

void initWaveformGenerator()
{
  pinMode(PWM_PIN, OUTPUT);
  setWaveform(WAVEFORM_SINUS);
  // Call the function to set default frequency, here you might want to specify a default frequency
  setWaveformFrequency(WAVEFORM_DEFAULT_FREQ_HZ);
  setWaveformDC(0);
  analogWrite(PWM_PIN, 0);
}

void setWaveform(uint8_t waveform0)
{
  if (waveform0 >= 0 and waveform0 < WAVEFORM_MAXWAVEFORM_NUM)
  {
    wf_wave0 = waveform0;
  } else {
    // Set default waveforms
    wf_wave0 = WAVEFORM_SINUS;
  }
}

void setWaveformFrequency(uint16_t frequency)
{
  if (frequency >= 1 and frequency < WF_FREQ_MAX_HZ)
  {
    wf_freq = frequency;
  } else {
    wf_freq = WAVEFORM_DEFAULT_FREQ_HZ;
  }
  wf_sample_us = 1000000UL / ((unsigned long)wf_freq * WAVEFORM_MAX_SAMPLES_NUM);
  
  if (frequency >= 30 && frequency <= 1000000)
  {
    setupPWMFrequency(PWM_PIN, frequency);
  }
}

void setWaveformDC(uint8_t dc)
{
  wf_dutyCycle = dc;
}

void enableWaveformOutput()
{
  wf_outputEnabled = true;
}

void disableWaveformOutput()
{
  wf_outputEnabled = false;
}

bool isWaveformEnabled()
{
  return wf_outputEnabled;
}

void pollWaveformGenerator()
{

  if (wf_outputEnabled)
  {

    if (wf_wave0 != WAVEFORM_DUTYCYCLE)
    {
      /*unsigned long currentMicros = micros();  // Aktuelle Zeit abrufen
      if (currentMicros - wf_prevMicros >= wf_sample_us)
      {
          wf_prevMicros = currentMicros;

          uint16_t sample = map(waveformsTable[wf_wave0][wf_pos], 0, 0xfff, 0, DAC_MAX_VOLTAGE);
          sample = constrain(sample, 0, DAC_MAX_VOLTAGE);*/
          // write the selected waveform on DAC0
          dac_setVoltage(MCP4725_I2C_ADDRESS, wf_pos*10); // do not write to EEPROM, 400000Hz DAC frequency

          //wf_pos++;
          wf_pos += 1;
          if (wf_pos >= WAVEFORM_MAX_SAMPLES_NUM)  // Reset the counter to repeat the wave
              wf_pos = 0;

      //}
    } else {
      // WAVEFORM_DUTYCYCLE
      if (analogRead(PWM_PIN) != wf_dutyCycle)
        analogWrite(PWM_PIN, wf_dutyCycle);
    }

    if (!wf_pwm_needs_disabling)
      wf_pwm_needs_disabling = true;

  } else {
    if (wf_pwm_needs_disabling)
    {
      dac_setVoltage(MCP4725_I2C_ADDRESS, 0);
      analogWrite(PWM_PIN, 0);
      wf_pwm_needs_disabling = false;
    }
  }
}

void setupPWMFrequency(uint8_t pin, unsigned long frequency) {
  byte prescalerbits = 0b001; // Voreinstellung: kein Prescaler (F_CPU)
  unsigned long prescaler_value;
  unsigned long ocr;

  // Berechne den idealen Prescaler für die gewünschte Frequenz
  prescaler_value = F_CPU / frequency;

  // Finde den passenden Prescaler
  if (prescaler_value < 65536UL) {
    prescalerbits = 0b001; // Kein Prescaler (F_CPU)
  }
  else if ((prescaler_value >>= 3) < 65536UL) {
    prescalerbits = 0b010; // Prescaler 8
  }
  else if ((prescaler_value >>= 3) < 65536UL) {
    prescalerbits = 0b011; // Prescaler 64
  }
  else if ((prescaler_value >>= 2) < 65536UL) {
    prescalerbits = 0b100; // Prescaler 256
  }
  else if ((prescaler_value >>= 2) < 65536UL) {
    prescalerbits = 0b101; // Prescaler 1024
  }
  else {
    prescalerbits = 0b101; // Prescaler 1024
    frequency = F_CPU / (prescaler_value = 65535UL);
  }

  // Berechne den Wert für den Output Compare Register (OCR)
  ocr = F_CPU / (prescaler_value * frequency) - 1;

  // Timer konfigurieren
  pinMode(pin, OUTPUT);
  if (pin == 5 || pin == 6) {
    TCCR0B &= ~0b111; // Prescaler löschen
    TCCR0B |= prescalerbits; // Neuen Prescaler setzen
    OCR0A = ocr; // Output Compare Register setzen
    TCCR0A |= (1 << WGM01); // CTC-Modus aktivieren
  }
  else if (pin == 9 || pin == 10) {
    TCCR1B &= ~0b111; // Prescaler löschen
    TCCR1B |= prescalerbits; // Neuen Prescaler setzen
    OCR1A = ocr; // Output Compare Register setzen
    TCCR1A |= (1 << WGM11); // CTC-Modus aktivieren
  }
  else if (pin == 3 || pin == 11) {
    TCCR2B &= ~0b111; // Prescaler löschen
    TCCR2B |= prescalerbits; // Neuen Prescaler setzen
    OCR2A = ocr; // Output Compare Register setzen
    TCCR2A |= (1 << WGM21); // CTC-Modus aktivieren
  }
}