#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
#include <Bounce2.h>

#include "./include/config.h"
#include "./include/screenids.h"
#include "./lib/debug.h"
#include "./lib/settings.h"
#include "./lib/screen.h"
#include "./lib/vl53l0x.h"
#include "./lib/Control.h"
#include "./lib/motor.h"
#include "./lib/control.h"
#include "./lib/state.h"
#include "./lib/screen/home.h"
#include "./lib/screen/calibrate.h"
#include "./lib/screen/manual.h"
#include "./lib/screen/move-overcurrent.h"
#include "./lib/screen/move-sensorerror.h"


enum class InitSequenceStep
{
  EEPROM = 0,
  HeightSensorInit,
  HeightSensorTest,

  Last = HeightSensorTest
};


// Forward declarations
inline void setupHeightSensor();
inline void testHeightSensor();

void initSequenceStart();
void initSequenceSuccess(InitSequenceStep step);
void initSequenceError(InitSequenceStep step);
void initSequenceDisplayHeight(uint16_t measurement);
void initSequenceEnd();


auto display = Adafruit_ST7789(Config::DisplayPinCS, Config::DisplayPinDC, Config::DisplayPinRST);
auto screenManager = ScreenManager(&display);

Bounce buttons[3];



/*

  Setup

*/
void setup()
{
  DebugInit();
  dln("[ MAIN     ] Debug log started");

  buttons[0].attach(Config::ButtonPinTop, INPUT_PULLUP);
  buttons[1].attach(Config::ButtonPinMiddle, INPUT_PULLUP);
  buttons[2].attach(Config::ButtonPinBottom, INPUT_PULLUP);

  display.init(Config::DisplayWidth, Config::DisplayHeight, SPI_MODE3);
  display.setRotation(Config::DisplayRotation);

  screenManager.init();

  initSequenceStart();
  motorInit();

  // Load settings from EEPROM
  auto initialized = readSettings();
  initSequenceSuccess(InitSequenceStep::EEPROM);


  // Initialize VL53L0X sensor
  CurrentTime = millis();

  setupHeightSensor();
  testHeightSensor();

  initSequenceEnd();


  CurrentTime = millis();


  if (initialized)
  {
    Control.snapToPreset();

    screenManager.show<HomeScreen>();
  }
  else
  {
    screenManager.show<CalibrateScreen>();
  }
}



inline void setupHeightSensor()
{
  auto error = Control.init();
  if (error != VL53L0XInitResult::Success)
  {
    initSequenceError(InitSequenceStep::HeightSensorInit);

    switch (error)
    {
      case VL53L0XInitResult::InvalidIdentification:
        display.print("Invalid identification");
        break;

      case VL53L0XInitResult::GetSpadInfoFailed:
        display.print("GetSpadInfo failed");
        break;

      case VL53L0XInitResult::VHVCalibrationFailed:
        display.print("VHV calibration failed");
        break;

      case VL53L0XInitResult::PhaseCalibrationFailed:
        display.print("Phase calibration failed");
        break;

      default:
        display.print("Unknown error");
        break;
    }

    while(1);
  }

  initSequenceSuccess(InitSequenceStep::HeightSensorInit);
}



inline void testHeightSensor()
{
  Control.stabilizeStart();

  while (true)
  {
    if (Control.update() != ControlUpdateResult::SensorError)
    {
      initSequenceDisplayHeight(Control.getCurrentMeasurement());

      if (Control.stabilized())
        break;
    }
    else
      initSequenceDisplayHeight(0);

    CurrentTime = millis();
  }

  initSequenceSuccess(InitSequenceStep::HeightSensorTest);
}



/*

  Loop

*/
void loop()
{
  CurrentTime = millis();


  switch (Control.update())
  {
    case ControlUpdateResult::TargetReached:
      switch (MoveReturnScreen)
      {
        case ScreenId::Manual:
          screenManager.show<ManualScreen>();
          break;

        default:
          screenManager.show<HomeScreen>();
          break;
      }
      break;

    case ControlUpdateResult::SensorError:
      screenManager.show<MoveSensorErrorScreen>(ScreenId::MoveSensorError);
      break;

    case ControlUpdateResult::Overcurrent:
      screenManager.show<MoveOvercurrentScreen>(ScreenId::MoveOvercurrent);
      break;

    default:
      break;
  }


  buttons[0].update();
  buttons[1].update();
  buttons[2].update();

  if (buttons[0].fell())
  {
    //dln("[ MAIN     ] Button pressed: Top");
    screenManager.button(Button::Top);
  }

  if (buttons[1].fell())
  {
    //dln("[ MAIN     ] Button pressed: Middle");
    screenManager.button(Button::Middle);
  }

  if (buttons[2].fell())
  {
    //dln("[ MAIN     ] Button pressed: Bottom");
    screenManager.button(Button::Bottom);
  }

  screenManager.tick();
}


/*

  Helper functions for the status display during the initialization sequence

*/
#define initSequenceTextSize 2

// Default font is 5x7
#define initSequenceTextY(step) ((1 + (uint8_t)step) * ((7 + 3) * initSequenceTextSize))


void initSequenceStart()
{
  display.fillScreen(Config::ColorInitSeqBackground);
  display.setTextSize(initSequenceTextSize);

  display.setCursor(0, 0);
  display.setTextColor(Config::ColorInitSeqTitle);
  display.println("Initializing...");

  display.setTextColor(Config::ColorInitSeqItems, Config::ColorInitSeqBackground);

  display.setCursor(0, initSequenceTextY(0));
  display.print("  reading EEPROM");

  display.setCursor(0, initSequenceTextY(1));
  display.print("  height sensor");

  display.setCursor(0, initSequenceTextY(2));
  display.print("  sensor test");
}


void initSequenceSuccess(InitSequenceStep step)
{

  display.drawChar(0, initSequenceTextY(step), 'v', Config::ColorInitSeqSuccess, Config::ColorInitSeqBackground, initSequenceTextSize);
}


void initSequenceError(InitSequenceStep step)
{
  display.drawChar(0, initSequenceTextY(step), 'x', Config::ColorInitSeqError, Config::ColorInitSeqBackground, initSequenceTextSize);
  display.setCursor(0, initSequenceTextY(InitSequenceStep::Last + 2));
}


void initSequenceDisplayHeight(uint16_t measurement)
{
  display.setCursor(0, initSequenceTextY(InitSequenceStep::Last + 2));

  if (measurement > 0)
  {
    display.print(measurement);
    display.print("mm     ");
  }
  else
    display.print("-          ");
}


void initSequenceEnd()
{
  display.setTextSize(1);
  delay(1000);
}