//#define DEBUG

#include <Arduino.h>
#include <avr/sleep.h>

#ifdef DEBUG
#include <SoftwareSerial.h>
#endif


/*

  Wiring:

                               +---u---+
                 10k R to +5V -| 1   8 |- +5V
  "Speed" potentiometer (RV3) -| 2   7 |- "Off" potentiometer (RV1)
     "On" potentiometer (RV2) -| 3   6 |- On/Off switch (active low)
                          GND -| 4   5 |- Servo PWM signal
                               +-------+
*/

static const uint8_t PinServo = 0;
static const uint8_t PinSwitch = 1;
static const uint8_t PinOffPosition = A1;
static const uint8_t PinOnPosition = A2;
static const uint8_t PinSpeed = A3;

static const float TransitionTimeMin = 1;
static const float TransitionTimeMax = 5000;
static const uint16_t SleepTime = 10000;

static const uint16_t ServoMinPulse = 544;
static const uint16_t ServoMaxPulse = 2400;


static const uint16_t ServoPulseInterval = 20;
static const uint16_t ServoStabilizeDelay = 100;


int offPosition;              // 0 - 180 degrees, read from "Off" potentiometer
int onPosition;               // 0 - 180 degrees, read from "On" potentiometer
int speed;                    // 0 - 1023, read from "Speed" potentiometer
float degreesPerMilli;        // Degrees per millisecond


bool lastOn;                  // If the switch was on when it was last flipped
int startPosition;            // The position the servo was in when the switch was flipped
unsigned long startTime;      // The last time the switch was flipped
unsigned long idleTime;       // The last time anything happened

bool servoEnabled;            // Whether or not pulses should be sent to the servo
int servoPosition;            // The desired position of the servo
unsigned long servoPulseTime; // The last time the servo pulse was sent


#ifdef DEBUG
SoftwareSerial debug(-1,3);
#endif


// Forward declarations
void pulseServo(unsigned long currentTime);
bool updateSettings();
inline int getNewPosition(unsigned long currentTime, int targetPosition) __attribute__((always_inline));
void sleepUntilSwitch();

float floatMap(float x, float in_min, float in_max, float out_min, float out_max);


void setup()
{
  #ifdef DEBUG
  debug.begin(9600);
  debug.println("RailroadSwitch starting");
  #endif

  pinMode(PinOffPosition, INPUT);
  pinMode(PinOnPosition, INPUT);
  pinMode(PinSwitch, INPUT_PULLUP);
  pinMode(PinServo, OUTPUT);

  offPosition = -1;
  onPosition = -1;
  speed = -1;

  updateSettings();

  lastOn = false;
  startPosition = offPosition;
  servoPosition = offPosition;

  servoPulseTime = 0;
  servoEnabled = true;

  #ifdef DEBUG
  debug.println("Started");
  #endif
}


void loop()
{
  unsigned long currentTime = millis();

  if (servoEnabled)
    pulseServo(currentTime);

  bool isOn = (digitalRead(PinSwitch) == LOW);

  // If the switch changed, start from the last known position
  if (isOn != lastOn)
  {
    #ifdef DEBUG
    debug.println("State changed");
    #endif

    lastOn = isOn;
    startPosition = servoPosition;
    startTime = currentTime;
    idleTime = startTime;
  }

  int targetPosition = isOn ? onPosition : offPosition;
  if (servoPosition != targetPosition)
  {
    servoPosition = getNewPosition(currentTime, targetPosition);
    servoEnabled = true;
    idleTime = currentTime;
  }
  else if (servoEnabled)
  {
    // Ensure we pulsed the last position
    currentTime = millis();

    delay(ServoPulseInterval);
    pulseServo(currentTime);
    delay(ServoPulseInterval);

    // Stop sending pulses when we reach the destination, to prevent
    // power consumption and possible buzzing
    servoEnabled = false;
    idleTime = currentTime;
  }


  // Don't update the settings while the servo is moving, as the
  // voltage drop can cause fluctuating results
  if (!servoEnabled && currentTime - idleTime >= ServoStabilizeDelay)
  {
    if (updateSettings())
    {
      // Immediately go to the new position
      currentTime = millis();
      servoPosition = isOn ? onPosition : offPosition;

      delay(ServoPulseInterval);
      pulseServo(currentTime);
      delay(ServoPulseInterval);

      idleTime = currentTime;
    }
  }

  // Stay awake for a bit to allow changes to the on and off positions
  // to take effect immediately
  if (currentTime - idleTime >= SleepTime)
    sleepUntilSwitch();
}


void pulseServo(unsigned long currentTime)
{
  if (currentTime - servoPulseTime < ServoPulseInterval)
    return;

  int pulseWidth = map(servoPosition, 0, 180, ServoMinPulse, ServoMaxPulse);

  // This will mess with the millis() result, but for what we're doing
  // that is acceptable to get a stable pulse.
  noInterrupts();

  digitalWrite(PinServo, HIGH);
  delayMicroseconds(pulseWidth);
  digitalWrite(PinServo, LOW);

  interrupts();


  servoPulseTime = currentTime;
}


bool updateSettings()
{
  int newOffPosition = map(analogRead(PinOffPosition), 0, 1023, 0, 180);
  int newOnPosition = map(analogRead(PinOnPosition), 0, 1023, 0, 180);
  int newSpeed = analogRead(PinSpeed);

  if (newOffPosition != offPosition || newOnPosition != onPosition || newSpeed != speed)
  {
    offPosition = newOffPosition;
    onPosition = newOnPosition;
    speed = newSpeed;

    float transitionTime = floatMap(speed, 0, 1023, TransitionTimeMin, TransitionTimeMax);
    degreesPerMilli = abs((float)(onPosition - offPosition)) / transitionTime;

    #ifdef DEBUG
    debug.println("Positions changed");
    debug.print("  Off position   : "); debug.println(offPosition);
    debug.print("  On position    : "); debug.println(onPosition);
    debug.print("  Transition time: "); debug.println(transitionTime);
    debug.print("  Degrees / milli: "); debug.println(degreesPerMilli);
    #endif

    return true;
  }

  return false;
}


int getNewPosition(unsigned long currentTime, int targetPosition)
{
  int newPosition;
  unsigned long timePassed = currentTime - startTime;

  if (targetPosition > startPosition)
  {
    newPosition = startPosition + ((float)timePassed * degreesPerMilli);
    if (newPosition > targetPosition)
      newPosition = targetPosition;
  }
  else
  {
    newPosition = startPosition - ((float)timePassed * degreesPerMilli);
    if (newPosition < targetPosition)
      newPosition = targetPosition;
  }

  #ifdef DEBUG
  debug.println("getNewPosition");
  debug.print("  Time passed    : "); debug.println(timePassed);
  debug.print("  Start position : "); debug.println(startPosition);
  debug.print("  Target position: "); debug.println(targetPosition);
  debug.print("  New position   : "); debug.println(newPosition);
  #endif

  return newPosition;
}


void sleepStart()
{
  // Turn ADC off
  ADCSRA &= ~_BV(ADEN);
  set_sleep_mode(SLEEP_MODE_PWR_DOWN);

  // Set sleep bit and halt the CPU
  sleep_enable();
  sei();
  sleep_cpu();

  // ...goooood morning!
  cli();
}


void sleepEnd()
{
  sleep_disable();
  ADCSRA |= _BV(ADEN);

  sei();
}


void sleepUntilSwitch()
{
  while (1)
  {
    // Enable pin change interrupts
    GIMSK |= _BV(PCIE);

    // Set up pin change mask
    PCMSK = digitalPinToBitMask(PinSwitch);

    sleepStart();
    PCMSK = 0;
    sleepEnd();
  }
}


float floatMap(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}