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

#include "./include/display.h"
#include "./include/heightsensor.h"
#include "./include/colors.h"
#include "./lib/persist.h"
#include "./lib/menu.h"
#include "./lib/vl53l0x.h"


// Forward declarations
void initSequenceStart();
void initSequenceSuccess(uint8_t which);
void initSequenceError(uint8_t which, const char* message);

#define INITSEQ_EEPROM 0
#define INITSEQ_HEIGHTSENSORINIT 1
#define INITSEQ_HEIGHTSENSORBUDGET 2
#define INITSEQ_HEIGHTSENSORTEST 3
#define INITSEQ_MAX INITSEQ_HEIGHTSENSORINIT



auto display = Adafruit_ST7789(DISPLAY_PORT_CS, DISPLAY_PORT_DC, DISPLAY_PORT_RST);
auto heightSensor = VL53L0X();

auto persist = Persist();
auto menu = Menu(&display);



void setup()
{
  display.init(DISPLAY_WIDTH, DISPLAY_HEIGHT, SPI_MODE3);
  display.setRotation(DISPLAY_ROTATION);

  initSequenceStart();


  // Load settings from EEPROM
  persist.init();
  initSequenceSuccess(INITSEQ_EEPROM);



  // Initialize VL53L0X sensor
  Wire.begin();

  VL53L0XResult result;
  if (!heightSensor.init(HEIGHTSENSOR_I2C_ADDRESS, &result))
  {
    initSequenceError(INITSEQ_HEIGHTSENSORINIT, "TODO: error message");
    while(1);
  }
  initSequenceSuccess(INITSEQ_HEIGHTSENSORINIT);


  VL53L0X_Error error;
  if (!heightSensor.setMeasurementTimingBudget(33000, &error))
  {
    initSequenceError(INITSEQ_HEIGHTSENSORBUDGET, "TODO: error message");
    while(1);
  }
  initSequenceSuccess(INITSEQ_HEIGHTSENSORBUDGET);


  // TODO height sensor test - are we getting stable values
  initSequenceSuccess(INITSEQ_HEIGHTSENSORTEST);


  delay(1000);


  // Show the main menu
  display.fillScreen(COLOR_MENU_BACKGROUND);
  menu.init();
}


void loop()
{
  // TODO
}



/*

  Helper functions for the status display during the initialization sequence

*/
// Default font is 5x7, x2 for the current text size
#define initSequenceTextY(which) ((1 + which) * 14)

void initSequenceStart()
{
  display.fillScreen(COLOR_INITSEQ_BACKGROUND);
  display.setTextSize(2);

  display.setCursor(0, 0);
  display.setTextColor(COLOR_INITSEQ_TITLE);
  display.println("Initializing...");

  display.setTextColor(COLOR_INITSEQ_ITEMS);
  display.println("  reading EEPROM");
  display.println("  height sensor init");
  display.println("  height sensor budget");
  display.println("  height sensor test");
}


void initSequenceSuccess(uint8_t which)
{

  display.drawChar(0, initSequenceTextY(which), 'v', COLOR_INITSEQ_SUCCESS, GREEN, 2);
}


void initSequenceError(uint8_t which, const char* message)
{
  display.drawChar(0, initSequenceTextY(which), 'x', COLOR_INITSEQ_ERROR, COLOR_INITSEQ_ERROR, 2);

  display.setCursor(0, initSequenceTextY(INITSEQ_MAX + 2));
  display.print(message);
}