#include <Arduino.h>
#include <EEPROM.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_VL53L1X.h>
#include "main.h"

uint16_t  offset = 0;
uint16_t  height = 0;
uint16_t  measurement = 0;
uint16_t  oldMeasurement = 0;
int16_t   delta = 0;
bool      updateScreen = 1;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, SCREEN_RESET);
Adafruit_VL53L1X vl53 = Adafruit_VL53L1X(XSHUT_PIN, IRQ_PIN);

void setup() {
  pinMode(RE_CLK, INPUT_PULLUP);
  pinMode(RE_DATA, INPUT_PULLUP);
  pinMode(RE_PRESS, INPUT_PULLUP);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);

  TWBR = 2; // force i2c clock freq to 400kHz

  digitalWrite(LED1, 1);
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    while (1)       delay(10);
  }
  display.clearDisplay();
  
  digitalWrite(LED2, 1);
  if (! vl53.begin(0x29, &Wire)) {
    while (1)       delay(10);
  }

  if (! vl53.startRanging()) {
    while (1)       delay(10);
  }
  vl53.setTimingBudget(500);

  digitalWrite(LED3, 1);
  EEPROM.get(EEPROM_ADDRESS, offset);
 
  digitalWrite(LED1, 0);
  digitalWrite(LED2, 0);
  digitalWrite(LED3, 0);
  
  startLoopDisplay();
}

void loop() {

  if (updateScreen) { // only ypdate the screen on changes
    updateScreen = 0;
    updateLoopDisplay();
  }

  if (vl53.dataReady()) {
    measurement = vl53.distance();
    vl53.clearInterrupt();
    if (measurement != oldMeasurement) {
      oldMeasurement = measurement;
      updateScreen = 1;
    }
  }

  if (digitalRead(RE_CLK) == 0) {     // turn the rotary encoder
    if (digitalRead(RE_DATA) == 0) {  // turn right
      if (height < 999) {
        height++;
      }
    } else {                          // turn left
      if (height > 0) {
        height--;
      }
    }
    updateScreen = 1;

    while (digitalRead(RE_CLK) == 0) {} // wait till encoder has rotated
  }

  if (digitalRead(RE_PRESS) == 0) {
    while (digitalRead(RE_PRESS) == 0) {} // wait till button is released
    config();
  }
}

void config() {
  bool doneConfig = 0;
  bool calibrate = 0;

  startConfigDisplay();
  updateConfigDisplay(calibrate);

  while (!doneConfig) {
    if (updateScreen) {
      updateScreen = 0;
      updateConfigDisplay(calibrate);
    }

    if (digitalRead(RE_CLK) == 0) {     // turn the rotary encoder
      if (digitalRead(RE_DATA) == 0) {  // turn right
        calibrate = 0;
        digitalWrite(LED2, 1);
      } else {                          // turn left
        calibrate = 1;
        digitalWrite(LED1, 1);
      }
      updateScreen = 1;

      while (digitalRead(RE_CLK) == 0) {} // wait till encoder has rotated
      digitalWrite(LED2, 0);
      digitalWrite(LED1, 0);
    }
    
    if (digitalRead(RE_PRESS) == 0) {
      digitalWrite(LED3, 1);

      if (calibrate) {
        while (!vl53.dataReady()) {
          delay(10);
        }
        offset = vl53.distance();
        vl53.clearInterrupt();
        EEPROM.put(EEPROM_ADDRESS, offset);
      }
      doneConfig = 1;
      while (digitalRead(RE_PRESS) == 0) {} // wait till button is released
      digitalWrite(LED3, 0);
    }
  }
  startLoopDisplay();
  updateScreen = 1;
}

void startLoopDisplay() {
  display.clearDisplay();
  display.setTextColor(1, 0);
  display.setTextSize(2);
  display.setFont(NULL);

  display.drawLine(73, 0, 73, 32, 1);
  display.display();
}

void updateLoopDisplay() {
  char      buffer[5];

  display.fillRect(4, 5, 19, 22, 0);
  if (measurement < (height + offset)) {
    display.fillRoundRect(11, 6, 5, 19, 3, 1);
    display.fillTriangle(4, 14, 22, 14, 13, 5, 1);
  }
  if (measurement == (height + offset)) {
  //  display.fillRoundRect(4, 13, 18, 5, 3, 1);
  }
  if (measurement > (height + offset)) {
    display.fillRoundRect(11, 6, 5, 19, 3, 1);
    display.fillTriangle(4, 17, 22, 17, 13, 26, 1);
  }

  delta = measurement - (height + offset);
  if (delta < 0) { delta = -delta; }
  if (delta > 999) { delta = 999; }
  sprintf(buffer,"%3d", delta);
  display.setCursor(30, 9);
  display.print(buffer);

  sprintf(buffer,"%3d", height);
  display.setCursor(84, 9);
  display.print(buffer);

  display.display();
}

void startConfigDisplay() {
  display.clearDisplay();
  display.setTextColor(1, 0);
  display.setTextSize(1);
  display.setFont(NULL);

  display.setCursor(16, 2);
  display.print(F("Calibrate offset"));

  display.drawLine(0, 12, 127, 12, 1);
  
  display.setTextSize(2);
  display.setCursor(64, 16);
  display.print("/");

  display.display();
}

void updateConfigDisplay(bool calibrate) {
  if (calibrate) {
    display.setCursor(16, 16);
    display.setTextColor(0, 1);
    display.print("YES");
    display.setCursor(90, 16);
    display.setTextColor(1, 0);
    display.print("NO");
  } else {
    display.setCursor(16, 16);
    display.setTextColor(1, 0);
    display.print("YES");
    display.setCursor(90, 16);
    display.setTextColor(0, 1);
    display.print("NO");
  }
  display.display();
}