#include "Arduino.h"
#include <LiquidCrystal.h>
#include <BasicStepperDriver.h>

#include "Scenerunner.h"

Scenerunner::Scenerunner(LiquidCrystal *lcd, BasicStepperDriver *stepper, int pin_focus, int pin_shutter, int pin_backlight) {
  _lcd = lcd;
  _stepper = stepper;
  _pin_focus = pin_focus;
  _pin_shutter = pin_shutter;
  _pin_backlight = pin_backlight;
}

void Scenerunner::start() {
  step = 1;
  _lcd->clear();
  _lcd->print("Step ");
  _lcd->print(step);
  runStep = RUN_SETTLE;
  runStepStart = millis();
}

bool Scenerunner::isBusy() {
  return runStep == RUN_OFF ? false : true;
}

void Scenerunner::_runHandler_off() {
  // don't do anything
}

void Scenerunner::_runHandler_settle() {
  _lcd->setCursor(0, 1);
  _lcd->print("Settling ");
  _lcd->print(tSettle);
  _lcd->print("ms");
  if (millis() >= runStepStart + tSettle) {
    _lcd->clear();
    _lcd->print("Step ");
    _lcd->print(step);
    runStep = RUN_FOCUS;
    runStepStart = millis();
    digitalWrite(_pin_focus, HIGH);
  }
}

void Scenerunner::_runHandler_focus() {
  _lcd->setCursor(0, 1);
  _lcd->print("Focusing ");
  _lcd->print(tFocus);
  _lcd->print("ms");
  if (millis() >= runStepStart + tFocus) {
    _lcd->clear();
    _lcd->print("Step ");
    _lcd->print(step);
    runStep = RUN_SHUTTER;
    runStepStart = millis();
    if (bDarkenLcd) {
      digitalWrite(_pin_backlight, LOW);
    }
    digitalWrite(_pin_shutter, HIGH);
  }
}

void Scenerunner::_runHandler_shutter() {
  _lcd->setCursor(0, 1);
  _lcd->print("Shutter ");
  _lcd->print(tShutter);
  _lcd->print("ms");
  if (millis() >= runStepStart + tShutter) {
    digitalWrite(_pin_focus, LOW);
    digitalWrite(_pin_shutter, LOW);
    digitalWrite(_pin_backlight, HIGH);
    if (step >= nSteps) {
      if (bReturn) {
        _lcd->clear();
        _lcd->print("Return to start");
        runStep = RUN_RETURN;
        runStepStart = millis();
        _stepper->startMove(- 200/8 * distance * (nSteps - 1));
      } else {
        _lcd->clear();
        _lcd->print("Done");
        runStep = RUN_OFF;
        runStepStart = millis();
      }
    } else {
      step++;
      _lcd->clear();
      _lcd->print("Step ");
      _lcd->print(step);
      runStep = RUN_MOVE;
      runStepStart = millis();
      _stepper->startMove(200/8 * distance);
    }
  }
}

void Scenerunner::_runHandler_move() {
  _lcd->setCursor(0, 1);
  _lcd->print("Moving ");
  _lcd->print(distance);
  _lcd->print("mm");
  if (_stepper->getStepsRemaining() == 0) {
    _lcd->clear();
    _lcd->print("Step ");
    _lcd->print(step);
    runStep = RUN_SETTLE;
    runStepStart = millis();
  }
}

void Scenerunner::_runHandler_return() {
  _lcd->setCursor(0, 1);
  _lcd->print("moving ");
  _lcd->print(distance * (nSteps - 1));
  _lcd->print("mm");
  if (_stepper->getStepsRemaining() == 0) {
    runStep = RUN_OFF;
    runStepStart = millis();
  }
}

void Scenerunner::nextAction() {
  switch (runStep) {
    case RUN_OFF:
      _runHandler_off();
      break;
    case RUN_SETTLE:
      _runHandler_settle();
      break;
    case RUN_FOCUS:
      _runHandler_focus();
      break;
    case RUN_SHUTTER:
      _runHandler_shutter();
      break;
    case RUN_MOVE:
      _runHandler_move();
      break;
    case RUN_RETURN:
      _runHandler_return();
      break;
  }
}
