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

// PIN A1  potentiometer
// PIN D2  direction in
// PIN D3  motor direction out
// PIN D10 <occupied by timer2>
// PIN D11 motor frequency out

#define POTI_PIN      A1
#define DIRECTION_PIN 2
#define MOTOR_FREQUENCY_PIN FREQUENCYTIMER2_PIN
#define MOTOR_DIRECTION_PIN 3

#define POT_JITTER_THRESHOLD 10

float fscale(int inputValue, float originalMin, float originalMax,
  float newBegin, float newEnd, float curve);

Bounce direction_button;

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

void setup() {
  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);

  Serial.begin(9600);
  FrequencyTimer2::setPeriod(7000);
  FrequencyTimer2::disable();
}

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 = analogRead(POTI_PIN);
  if (abs(newPotValue - target.potValue) > POT_JITTER_THRESHOLD)
  {
    target.potValue = newPotValue;
    target.period   = (int)fscale(target.potValue / 10, 0, 102, 120, 7000, -5);
    if (target.period > 6500)
      target.period = 7000;
    Serial.print("target period: ");
    Serial.println(target.period, DEC);
  }

  const int step_delay = 200;
  if (current.period != target.period || current.direction != target.direction)
  {
    if (!timer_enabled && target.period != 7000)
    {
      FrequencyTimer2::enable();
      timer_enabled = true;
      Serial.println("enable motor");
      delay(step_delay);
    }

    /*int step = (current.potValue < 20) ? 1
      : (current.potValue < 100) ? 2
      : (current.potValue < 300) ? 3
      : (current.potValue < 400) ? 5
      : (current.potValue < 500) ? 10
      : 20;*/
    int step = (current.period < 150) ? 1
      : (current.period < 200)  ? 2
      : (current.period < 250)  ? 3
      : (current.period < 300)  ? 5
      : (current.period < 400)  ? 10
      : (current.period < 500)  ? 25
      : (current.period < 1000) ? 50
      : (current.period < 1500) ? 100
      : (current.period < 2000) ? 300
      : (current.period < 3000) ? 500
      : 1000;
    int period = (current.direction == target.direction) ? target.period : 7000;
    step = min(abs(period - current.period), step);
    if (period < current.period)
      step *= -1;
    current.period += step;

    FrequencyTimer2::setPeriod(current.period);
    //Serial.print("current potValue: ");
    //Serial.print(current.potValue, DEC);
    Serial.print(" current period: ");
    Serial.println(current.period, DEC);
    delay(step_delay);

    if (current.period == 7000 && current.direction != target.direction)
    {
      FrequencyTimer2::disable();
      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 && analogRead(POTI_PIN) == 1023)
  {
    FrequencyTimer2::disable();
    timer_enabled = false;
    Serial.println("disable motor");
    delay(step_delay);
  }

#if 0
  static unsigned long v = 0;
  if (Serial.available()) {
    char ch = Serial.read();
    switch(ch) {
      case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 'p':
        FrequencyTimer2::setPeriod(v);
        Serial.print("set ");
        Serial.print((long)v, DEC);
        Serial.print(" = ");
        Serial.print((long)FrequencyTimer2::getPeriod(), DEC);
        Serial.println();
        v = 0;
        break;
      case 'e':
        FrequencyTimer2::enable();
        break;
      case 'd':
        FrequencyTimer2::disable();
        break;
        break;
      case 'n':
        FrequencyTimer2::setOnOverflow(0);
        break;
    }
  }
#endif
}

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;
}