#include <Arduino.h>
#include <Bounce2.h>

// relais board:
// A1  = potentiometer
// SDA = 24V+
// SCL = direction in

// PIN A1 potentiometer
// PIN 0
// PIN 1  relay 1
// PIN 2  relay 2
// PIN 3
// PIN 4  direction in
// PIN 5  motor frequency out
// PIN 6  motor direction out
// PIN 7  CAN interrupt

#define POTI_PIN      1
#define DIRECTION_PIN 4
#define MOTOR_FREQUENCY_PIN 5 // hardcoded in SetupTimer()
#define MOTOR_DIRECTION_PIN 6

#define POT_JITTER_THRESHOLD 20

void SetupTimer();
void StopTimer();
void RestartTimer();
void SetFrequencyOutput(uint16_t frequency);
void SetMotorOutput(uint16_t rpm);
float fscale(int inputValue, float originalMin, float originalMax,
  float newBegin, float newEnd, float curve);

Bounce direction_button;
const int frequency_max = 6000;

struct {
  int direction = 0;
  int frequency = 0;
  int potValue  = 1023;
} current, target;
bool timer_enabled = false;

void setup()
{
  Serial.begin(9600);
  pinMode(MOTOR_FREQUENCY_PIN, OUTPUT);

  direction_button.attach(DIRECTION_PIN, INPUT_PULLUP);
  target.direction = current.direction = !direction_button.read();
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);
  digitalWrite(MOTOR_DIRECTION_PIN, target.direction);

  SetupTimer();
}

void loop()
{
  direction_button.update();
  if (direction_button.changed())
  {
    target.direction = !direction_button.read();
    Serial.print("target direction: ");
    Serial.println(target.direction, DEC);
  }

  int newPotValue = 1023 - analogRead(POTI_PIN); // inverse input values
  if (newPotValue < 30)
    newPotValue = 0;
  else if (newPotValue > 1000)
    newPotValue = 1023;
  if (abs(newPotValue - target.potValue) > POT_JITTER_THRESHOLD)
  {
    target.potValue = newPotValue;
    Serial.print("newPotValue: ");
    Serial.println(newPotValue, DEC);
    //target.frequency = (int)fscale(target.potValue / 10, 0, 102, 0, frequency_max, -4);
    target.frequency = map(target.potValue / 10, 0, 102, 0, frequency_max);
    Serial.print("target frequency: ");
    Serial.println(target.frequency, DEC);
  }

  const int step_delay = 200;
  if (current.frequency != target.frequency || current.direction != target.direction)
  {
    if (!timer_enabled && target.frequency != 0)
    {
      RestartTimer();
      timer_enabled = true;
      Serial.println("enable motor");
      delay(step_delay);
    }

    int step = (current.frequency > 5000) ? 100
      : (current.frequency > 4000) ? 100
      : (current.frequency > 3000) ? 150
      : (current.frequency > 2000) ? 150
      : (current.frequency > 1000) ? 120
      : 200;
    int frequency = (current.direction == target.direction) ? target.frequency : 0;
    step = min(abs(frequency - current.frequency), step);
    if (frequency < current.frequency)
      step *= -1;
    current.frequency += step;

    SetFrequencyOutput(current.frequency);
    //Serial.print("current potValue: ");
    //Serial.print(current.potValue, DEC);
    Serial.print("current frequency: ");
    Serial.println(current.frequency, DEC);
    delay(step_delay);

    if (current.frequency == 0 && current.direction != target.direction)
    {
      StopTimer();
      timer_enabled = false;
      Serial.println("disable motor");

      current.direction = target.direction;
      digitalWrite(MOTOR_DIRECTION_PIN, target.direction);
      Serial.print("changed direction to: ");
      Serial.println(current.direction, DEC);

      delay(step_delay);
    }
  }
  else if (timer_enabled && newPotValue == 0)
  {
    StopTimer();
    timer_enabled = false;
    Serial.println("disable motor");
    delay(step_delay);
  }
}

void SetupTimer()
{
  GCLK->GENDIV.reg = GCLK_GENDIV_DIV(1) |          // Divide the 48MHz clock source by divisor 1: 48MHz/1=48MHz
                     GCLK_GENDIV_ID(4);            // Select Generic Clock (GCLK) 4
  while (GCLK->STATUS.bit.SYNCBUSY);               // Wait for synchronization

  GCLK->GENCTRL.reg = GCLK_GENCTRL_IDC |           // Set the duty cycle to 50/50 HIGH/LOW
                      GCLK_GENCTRL_GENEN |         // Enable GCLK4
                      GCLK_GENCTRL_SRC_DFLL48M |   // Set the 48MHz clock source
                      GCLK_GENCTRL_ID(4);          // Select GCLK4
  while (GCLK->STATUS.bit.SYNCBUSY);               // Wait for synchronization

  // Enable the port multiplexer for port D5
  PORT->Group[g_APinDescription[5].ulPort].PINCFG[g_APinDescription[5].ulPin].bit.PMUXEN = 1;

  // Connect the TCC timers to the port outputs - port pins are paired odd PMUO and even PMUXE
  // F & E specify the timers: TCC0, TCC1 and TCC2
  PORT->Group[g_APinDescription[5].ulPort].PMUX[g_APinDescription[5].ulPin >> 1].reg = PORT_PMUX_PMUXO_F;

  // Feed GCLK4 to TCC0 and TCC1
  GCLK->CLKCTRL.reg = GCLK_CLKCTRL_CLKEN |         // Enable GCLK4 to TCC0 and TCC1
                      GCLK_CLKCTRL_GEN_GCLK4 |     // Select GCLK4
                      GCLK_CLKCTRL_ID_TCC0_TCC1;   // Feed GCLK4 to TCC0 and TCC1
  while (GCLK->STATUS.bit.SYNCBUSY);                // Wait for synchronization

  TCC0->WAVE.reg = TCC_WAVE_WAVEGEN_NPWM;          // Single slope PWM operation
  while (TCC0->SYNCBUSY.bit.WAVE);                 // Wait for synchronization

  TCC0->PER.reg = 4799;                            // Set the frequency of the PWM on TCC0 to 10kHz
  while (TCC0->SYNCBUSY.bit.PER);                  // Wait for synchronization

  TCC0->CC[2].reg = 2399;                          // TCC0 CC2 - 50% duty-cycle on D10
  while (TCC0->SYNCBUSY.bit.CC2);                  // Wait for synchronization
  TCC0->CC[3].reg = 2399;                          // TCC0 CC3 - 50% duty-cycle on D12
  while (TCC0->SYNCBUSY.bit.CC3);                  // Wait for synchronization

  // Divide the 48MHz signal by 1 giving 48MHz (20.83ns) TCC0 timer tick and enable the timer
  TCC0->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV1 |    // Divide GCLK4 by 1
                     TCC_CTRLA_ENABLE;             // Enable the TCC0 output
  while (TCC0->SYNCBUSY.bit.ENABLE);               // Wait for synchronization

  //Stop TCC0 counter on MCU initialization (stepper initially stopped)
  TCC0->CTRLBSET.reg = TCC_CTRLBSET_CMD_STOP;    // Force the TCC0 timer to STOP
  while (TCC0->SYNCBUSY.bit.CTRLB);              // Wait for synchronization
}

void StopTimer()
{
  //Stop TCC0 counter on MCU initialization (stepper initially stopped)
  TCC0->CTRLBSET.reg = TCC_CTRLBSET_CMD_STOP;           // Force the TCC0 timer to STOP
  while (TCC0->SYNCBUSY.bit.CTRLB);                     // Wait for synchronization
}

void RestartTimer()
{
  // put your main code here, to run repeatedly:
  TCC0->CTRLBSET.reg = TCC_CTRLBSET_CMD_RETRIGGER;      // Force the TCC0 timer to START
  while (TCC0->SYNCBUSY.bit.CTRLB);                     // Wait for synchronization
}

void SetFrequencyOutput(uint16_t frequency)
{
  uint32_t stepperFrequencyDivisor = round(48000000 / frequency);

  TCC0->PERB.reg = stepperFrequencyDivisor - 1;         // Set the starting frequency of the PWM on TCC0 to 1Hz
  while (TCC0->SYNCBUSY.bit.PERB);                      // Wait for synchronization

  TCC0->CCB[1].reg = (stepperFrequencyDivisor / 2) - 1; // TCC0 CC2 - 50% duty-cycle
  while (TCC0->SYNCBUSY.bit.CCB2);                      // Wait for synchronization
}

void SetMotorOutput(uint16_t rpm)
{
  const float stepperHzToRPM = 1;                      //6.67hz runs the stepper at 1 RPM
  SetFrequencyOutput(rpm * stepperHzToRPM);
}

float fscale(int inputValue, float originalMin, float originalMax,
  float newBegin, float newEnd, float curve)
{
  // condition curve parameter
  curve = constrain(curve, -10, 10);
  curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output
  curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function

  // Check for out of range inputValues
  inputValue = constrain(inputValue, originalMin, originalMax);

  // Check for originalMin > originalMax  - the math for all other cases i.e. negative numbers seems to work out fine
  if (originalMin > originalMax)
    return 0;

  // Zero Refference the values
  float originalRange = originalMax - originalMin;
  float zeroRefCurVal = inputValue - originalMin;
  float normalizedCurVal = zeroRefCurVal / originalRange;   // normalize to 0 - 1 float

  float rangedValue = 0;
  if (newEnd > newBegin)
  {
    float newRange = newEnd - newBegin;
    rangedValue = (pow(normalizedCurVal, curve) * newRange) + newBegin;
  }
  else
  {
    float newRange = newBegin - newEnd;
    rangedValue = newBegin - (pow(normalizedCurVal, curve) * newRange);
  }
  return rangedValue;
}