#include <Arduino.h>
#include <Bounce2.h>
#include <WiFi.h>

#include <esp_wifi.h>
#include <esp_bt.h>
#include <soc/soc.h>
#include <soc/rtc_cntl_reg.h>

#include "myhub.h"

const uint8_t BUTTON_BASE_1   = 25;
const uint8_t BUTTON_BASE_2   = 22;
const uint8_t BUTTON_BASE_3   = 26;
const uint8_t BUTTON_BASE_4   = 18;
const uint8_t BUTTON_BASE_5   = 33;
const uint8_t BUTTON_BASE_6   = 5;
const uint8_t BUTTON_BASE_7   = 19;
const uint8_t BUTTON_BASE_8   = 23;
const uint8_t BUTTON_HANDLE_1 = 17;
const uint8_t BUTTON_HANDLE_2 = 16;
const uint8_t POTI_THROTTLE   = 32;

const int idle_shutdown = 10 * 60 * 1000;
unsigned long last_scan_start = 0;

Bounce2::Button button_base1   = Bounce2::Button();
Bounce2::Button button_base2   = Bounce2::Button();
Bounce2::Button button_base3   = Bounce2::Button();
Bounce2::Button button_base4   = Bounce2::Button();
Bounce2::Button button_base5   = Bounce2::Button();
Bounce2::Button button_base6   = Bounce2::Button();
Bounce2::Button button_base7   = Bounce2::Button();
Bounce2::Button button_base8   = Bounce2::Button();
Bounce2::Button button_handle1 = Bounce2::Button();
Bounce2::Button button_handle2 = Bounce2::Button();

MyHub myHub;

void color_sensor_callback(void *hub, byte port_number,
  DeviceType device_type, uint8_t *data)
{
  MyHub *myHub = (MyHub *)hub;

  if (device_type == DeviceType::DUPLO_TRAIN_BASE_COLOR_SENSOR)
  {
    int color = myHub->parseColor(data);
    Serial.print("Color: ");
    Serial.println(color);
  }
}

void speedometer_sensor_callback(void *hub, byte port_number,
  DeviceType device_type, uint8_t *data)
{
  MyHub *myHub = (MyHub *)hub;

  if (device_type == DeviceType::DUPLO_TRAIN_BASE_SPEEDOMETER)
  {
    int current_speed = myHub->parseSpeedometer(data);
    Serial.print("speedometer="); Serial.println(current_speed);
    if (current_speed == 0)
    {
      //TODO child has fun with "crashing" the train into things
      // and the train reacts with the break sound
      //myHub->stopMotor();
      myHub->stopMotorWithBreak();
    }
    else if (myHub->isMotorStopped())
    {
      if (current_speed > 10)
        myHub->setMotorSpeedLevel(1);
      else if (current_speed < -10)
        myHub->setMotorSpeedLevel(-1);
    }
  }
}

#ifdef WIFI_TOGGLE
enum wifi_enable_state
  : uint8_t
{
  Enable  = 0,
  Disable = 1,
  None    = 2,
};

hw_timer_t *timer = NULL;

volatile wifi_enable_state wifi_enable = wifi_enable_state::None;
void IRAM_ATTR on_timer()
{
  static volatile int cnt = 0;
  cnt = (cnt < 15) ? cnt + 1 : 0;
  if (cnt == 0)
    wifi_enable = wifi_enable_state::Enable;
  else if (cnt == 1)
    wifi_enable = wifi_enable_state::Disable;
}
#endif

void controller_deep_sleep()
{
  Serial.print("Controller deep sleep enabled");

#ifdef WIFI_TOGGLE
  WiFi.disconnect(true);
  WiFi.mode(WIFI_OFF);
  esp_wifi_stop();
#endif

  NimBLEDevice::getScan()->stop();
  NimBLEDevice::deinit();
  esp_bt_controller_disable();

  // disable brownout detector
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
  //adc_power_release();
  esp_deep_sleep_start();
}

void setup()
{
  // disable brownout detector
  // problems with the usb battery?
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);

  Serial.begin(115200);
  pinMode(BUILTIN_LED, OUTPUT);

  analogReadResolution(12); //12 bits
  analogSetAttenuation(ADC_0db);  //For all pins
  analogSetPinAttenuation(POTI_THROTTLE, ADC_11db); //11db attenuation

  button_base1.attach(BUTTON_BASE_1, INPUT_PULLUP);
  button_base2.attach(BUTTON_BASE_2, INPUT_PULLUP);
  button_base3.attach(BUTTON_BASE_3, INPUT_PULLUP);
  button_base4.attach(BUTTON_BASE_4, INPUT_PULLUP);
  button_base5.attach(BUTTON_BASE_5, INPUT_PULLUP);
  button_base6.attach(BUTTON_BASE_6, INPUT_PULLUP);
  button_base7.attach(BUTTON_BASE_7, INPUT_PULLUP);
  button_base8.attach(BUTTON_BASE_8, INPUT_PULLUP);
  button_handle1.attach(BUTTON_HANDLE_1, INPUT_PULLUP);
  button_handle2.attach(BUTTON_HANDLE_2, INPUT_PULLUP);

  button_base1.setPressedState(LOW);
  button_base2.setPressedState(LOW);
  button_base3.setPressedState(LOW);
  button_base4.setPressedState(LOW);
  button_base5.setPressedState(LOW);
  button_base6.setPressedState(LOW);
  button_base7.setPressedState(LOW);
  button_base8.setPressedState(LOW);
  button_handle1.setPressedState(LOW);
  button_handle2.setPressedState(LOW);

  last_scan_start = 0;
  myHub.init();

  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_DEFAULT);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL0);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL1);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL2);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL3);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL4);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL5);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL6);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL7);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL8);

#ifdef WIFI_TOGGLE
  timer = timerBegin(0, 80, true); // 1 tick = 1/(80MHZ) => x/80 = 1us
  timerAttachInterrupt(timer, &on_timer, true);
  timerAlarmWrite(timer, 1000000, true); // 1000000 * 1us = 1sec
  timerAlarmEnable(timer);
#endif
}

void loop()
{
  /*
   * TODO
   * - color sensor
   * - throttle
   */
  if (!myHub.isConnecting() && !myHub.isConnected())
  {
    if (last_scan_start == 0)
    {
      delay(1000); // avoid fast-reconnect loops
      last_scan_start = millis();
    }
    if (millis() - last_scan_start > idle_shutdown)
      (void)controller_deep_sleep(); // never returns

    if (!NimBLEDevice::getScan()->isScanning())
    {
      myHub.init();
      Serial.println("BLE is scanning...");
    }
  }
  else if (myHub.isConnecting())
  {
    Serial.println("Duplo Hub found");
    myHub.connectHub();
    if (myHub.isConnected())
    {
      last_scan_start = 0;

      Serial.println("Connected to Duplo Hub");
      myHub.onConnect();
      delay(200);

      // connect speed sensor and activate it for updates
      myHub.activatePortDevice((byte)DuploTrainHubPort::SPEEDOMETER,
        speedometer_sensor_callback);
      delay(200);

      // activate light
      myHub.activateRgbLight();
      delay(200);

      // activate speaker
      myHub.activateBaseSpeaker();
      delay(200);
    }
    else
    {
      Serial.println("Failed to connect to Duplo Hub");
      return;
    }
  }
  else if (myHub.isConnected())
  {
    button_base1.update();
    if (button_base1.pressed())
    {
      Serial.println("Button1 pressed");
      myHub.playSound((byte)DuploTrainBaseSound::STEAM);
    }

    button_base2.update();
    if (button_base2.pressed())
    {
      Serial.println("Button2 - brake pressed");
      myHub.stopMotorWithBreak();
    }

    button_base3.update();
    if (button_base3.pressed())
    {
      Serial.println("Button3 - light color pressed");
      myHub.cycleLightColor();
    }

    button_base4.update();
    if (button_base4.pressed())
    {
      Serial.println("Button4 - light toggle pressed");
      myHub.toggleLight();
    }

    button_base5.update();
    if (button_base5.pressed())
    {
      Serial.println("Button5 - refill pressed");
      myHub.playSound((byte)DuploTrainBaseSound::WATER_REFILL);
    }

    button_base6.update();
    if (button_base6.pressed())
    {
      Serial.println("Button6 - horn pressed");
      myHub.playSound((byte)DuploTrainBaseSound::HORN);
    }

    button_base7.update();
    if (button_base7.pressed())
    {
      Serial.println("Button7 - brake pressed");
      myHub.stopMotorWithBreak();
    }

    button_base8.update();
    if (button_base8.pressed())
    {
      Serial.println("Button8 pressed");
      myHub.playSound((byte)DuploTrainBaseSound::STATION_DEPARTURE);
    }

    button_handle1.update();
    if (button_handle1.pressed())
    {
      Serial.println("Button Handle1 - speed++");
      myHub.incrMotorSpeed();
    }

    button_handle2.update();
    if (button_handle2.pressed())
    {
      Serial.println("Button Handle2 - speed--");
      myHub.decrMotorSpeed();
    }

    if (myHub.isMotorStopped() && millis() - myHub.millisLastCommand() > idle_shutdown)
    {
      myHub.shutDownHub();
      delay(500);
      myHub.shutDownHub();
      delay(500);

      (void)controller_deep_sleep(); // never returns
    }

#if 0
    int potValue = analogRead(POTI_THROTTLE);
    delay(50);
    int potValue2 = analogRead(POTI_THROTTLE);
    if (potValue == potValue2) {
      //if (potValue <= )
      Serial.print(" pot value "); Serial.println(potValue);
    }
    int speedValue = map(potValue, 0, 4095, -2, 2);
    if (potValue == potValue2 && speed != speedValue)
    {
      Serial.print("old speed "); Serial.print(speed);
      Serial.print(" new speed "); Serial.print(speedValue);
      Serial.print("New speed "); Serial.println(speedValue * 35);
      //myHub.setBasicMotorSpeed(motorPort, speedValue * 35);
      speed = speedValue;
    }
    delay(50);
#endif
  }

#ifdef WIFI_TOGGLE
  if (wifi_enable != wifi_enable_state::None)
  {
    timerAlarmDisable(timer);
    Serial.print("new mode ");
    Serial.println((wifi_enable == wifi_enable_state::Enable) ? "WIFI_STA" : "WIFI_OFF");
    WiFi.mode((wifi_enable == wifi_enable_state::Enable) ? WIFI_STA : WIFI_OFF);
    digitalWrite(BUILTIN_LED, (wifi_enable == wifi_enable_state::Enable) ? HIGH : LOW);
    wifi_enable = wifi_enable_state::None;
    timerAlarmEnable(timer);
  }
#endif
}