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

#include "./include/config.h"
#include "./lib/debug.h"
#include "./lib/settings.h"
#include "./lib/screen.h"
#include "./lib/vl53l0x.h"
#include "./lib/state.h"
#include "./lib/motor.h"
#include "./lib/motorstate.h"
#include "./lib/screen/home.h"


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

  Last = HeightSensorTest
};


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

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

bool heightSensorGetRange(uint16_t* measurement);


auto display = Adafruit_ST7789(Config::DisplayPortCS, Config::DisplayPortDC, Config::DisplayPortRST);
auto heightSensor = VL53L0X();

auto screenManager = ScreenManager(&display);

Bounce buttons[3];


/*

  Setup

*/
void setup()
{
  DebugInit();

  buttons[0].attach(Config::ButtonPortUp, INPUT_PULLUP);
  buttons[1].attach(Config::ButtonPortMenu, INPUT_PULLUP);
  buttons[2].attach(Config::ButtonPortDown, INPUT_PULLUP);

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

  screenManager.init();

  initSequenceStart();

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


  // Initialize VL53L0X sensor
  setupHeightSensor();
  auto currentHeight = testHeightSensor();

  initSequenceEnd();


  State.CurrentTime = millis();

  if (initialized)
  {
    State.CurrentHeight = currentHeight;
    screenManager.show<HomeScreen>();
  }
  else
    // TODO show height configuration screen
    screenManager.show<HomeScreen>();
}



inline void setupHeightSensor()
{
  Wire.begin();

  heightSensor.setTimeout(500);
  auto error = heightSensor.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);
  }

  heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);

  initSequenceSuccess(InitSequenceStep::HeightSensorInit);
}



inline uint16_t testHeightSensor()
{
  uint16_t reference = 0;
  uint8_t closeCount = 0;
  uint16_t measurement;

  while (closeCount < 3)
  {
    if (heightSensorGetRange(&measurement))
    {
      initSequenceDisplayHeight(measurement);

      if (abs(measurement - reference) <= Config::HeightMeasurementDeltaStable)
        closeCount++;
      else
      {
        reference = measurement;
        closeCount = 0;
      }
    }
    else
    {
      initSequenceDisplayHeight(0);

      reference = 0;
      closeCount = 0;
    }

    delay(500);
  }

  initSequenceSuccess(InitSequenceStep::HeightSensorTest);
  return reference;
}



/*

  Loop

*/
// Forward declarations
void updateHeight();



void loop()
{
  State.CurrentTime = millis();

  if (State.MoveDirection != Direction::None)
  {
    if (motorStateCheckOverCurrent())
    {
      // TODO go to overcurrent screen
    }
    else
      updateHeight();
  }

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


  if (buttons[0].rose())
    screenManager.button(Button::Up);

  if (buttons[1].rose())
    screenManager.button(Button::Menu);

  if (buttons[2].rose())
    screenManager.button(Button::Down);

  screenManager.tick();
}



uint32_t lastValidMeasurement;

void updateHeight()
{
  uint16_t measurement;

  if (heightSensorGetRange(&measurement))
  {
    State.CurrentHeight = measurement;
    lastValidMeasurement = State.CurrentTime;

    if (motorStateCheckTargetReached())
      screenManager.show<HomeScreen>();
  }
  else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
  {
    motorStateStop();

    // TODO go to height sensor error screen
  }
}


bool heightSensorGetRange(uint16_t* measurement)
{
  *measurement = heightSensor.readRangeSingleMillimeters();
  return !heightSensor.timeoutOccurred() && *measurement <= Config::HeightMeasurementMax;
}



/*

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