#include "Watchy.h"
#include "bma423.h"

WatchyDisplayBase Watchy::m_displayBase;
WatchyDisplay Watchy::m_display(Watchy::m_displayBase);
WatchyRTC Watchy::m_RTC;

RTC_DATA_ATTR BMA423 Watchy::m_sensor;
RTC_DATA_ATTR bool g_displayFullInit = true;

Watchy::Watchy()
{
}

void Watchy::Init()
{
  esp_sleep_wakeup_cause_t wakeup_reason;
  wakeup_reason = esp_sleep_get_wakeup_cause();

  Wire.begin(SDA, SCL);
  m_RTC.init();

  m_display.epd2.selectSPI(SPI, SPISettings(20000000, MSBFIRST, SPI_MODE0));
  m_display.init(0, g_displayFullInit, 10, true);
  SetBusyCallback();

  Setup();

  switch(wakeup_reason) {
    case ESP_SLEEP_WAKEUP_EXT0:
    {
      // RTC interrupt
      ShowWatchFace(true);
      break;
    }
    case ESP_SLEEP_WAKEUP_EXT1:
    {
      uint64_t wakeupBit = esp_sleep_get_ext1_wakeup_status();
      if (wakeupBit & ACC_INT_MASK) {
        m_sensor.getINT();
        uint8_t irqMask = m_sensor.getIRQMASK();

        if(irqMask & BMA423_TILT_INT) {
          HandleTilt();
        }
        
        if(irqMask & BMA423_WAKEUP_INT) {
          HandleDoubleTap();
        }

        m_sensor.getINT();
      } else if (wakeupBit & BTN_PIN_MASK) {

        // Button press
        HandleButtonPress(wakeupBit);
        break;
      }

      // Button press
      HandleButtonPress(wakeupBit);
      break;
    }
    default:
    {
      BmaConfig();
      ConnectWiFi();
      SyncNTPTime();
      DisconnectWiFi();

      ShowWatchFace(false);
      break;
    }
  }

  DeepSleep();
}

void Watchy::DeepSleep()
{
  m_display.hibernate();
  if (g_displayFullInit) { // For some reason, seems to be enabled on first boot
    esp_sleep_disable_wakeup_source(ESP_SLEEP_WAKEUP_ALL);
  }

  g_displayFullInit = false; // Notify not to init it again
  m_RTC.clearAlarm();        // resets the alarm flag in the RTC

  // Set GPIOs 0-39 to input to avoid power leaking out
  const uint64_t ignore = 0b11110001000000110000100111000010; // Ignore some GPIOs due to resets
  for (int i = 0; i < GPIO_NUM_MAX; i++) {
    if ((ignore >> i) & 0b1) {
      continue;
    }
    pinMode(i, INPUT);
  }
  esp_sleep_enable_ext0_wakeup((gpio_num_t)RTC_INT_PIN, 0); // enable deep sleep wake on RTC interrupt
  esp_sleep_enable_ext1_wakeup(
    BTN_PIN_MASK | ACC_INT_MASK,
    ESP_EXT1_WAKEUP_ANY_HIGH);
  esp_deep_sleep_start();
}

void Watchy::DisplayBusyCallback(const void *)
{
  gpio_wakeup_enable((gpio_num_t)DISPLAY_BUSY, GPIO_INTR_LOW_LEVEL);
  esp_sleep_enable_gpio_wakeup();
  esp_light_sleep_start();
}

void Watchy::VibeMotor(uint8_t intervalMs, uint8_t length)
{
  pinMode(VIB_MOTOR_PIN, OUTPUT);
  bool motorOn = false;
  for (int i = 0; i < length; i++) {
    motorOn = !motorOn;
    digitalWrite(VIB_MOTOR_PIN, motorOn);
    delay(intervalMs);
  }
}

float Watchy::GetBatteryVoltage()
{
  return analogReadMilliVolts(BATT_ADC_PIN) / 1000.0f * 2.0f;
}

uint64_t Watchy::GetSteps()
{
  return m_sensor.getCounter();
}

void Watchy::ResetSteps()
{
  m_sensor.resetStepCounter();
}

void Watchy::ConnectWiFi()
{
  if(WiFi.begin(WIFI_SSID, WIFI_PASS) == WL_CONNECT_FAILED) {  
    Serial.begin(9600);
    Serial.println("Failed to connect to WiFi");
    return;
  }

  if(WiFi.waitForConnectResult() != WL_CONNECTED) {
    Serial.begin(9600);
    Serial.println("Failed to connect to WiFi");
    return;
  }
}

void Watchy::SyncNTPTime()
{
  WiFiUDP ntpUDP;
  // GMT offset should be 0, RTC class will adjust to local time
  NTPClient timeClient(ntpUDP, NTP_SERVER, 0);
  timeClient.begin();
  bool success = timeClient.forceUpdate();
  if (success) {
    tmElements_t tm;
    breakTime((time_t)timeClient.getEpochTime(), tm);
    m_RTC.set(tm);
  } else {
    Serial.begin(9600);
    Serial.println("Failed to get NTP time");
  }
}

void Watchy::DisconnectWiFi()
{
  WiFi.mode(WIFI_OFF);
}

void Watchy::ShowWatchFace(bool partialRefresh)
{
  DrawWatchFace(partialRefresh);
}

void Watchy::ClearBusyCallback()
{
  m_display.epd2.setBusyCallback(nullptr);
}

void Watchy::SetBusyCallback()
{
  m_display.epd2.setBusyCallback(DisplayBusyCallback);
}

uint16_t Watchy::ReadRegister(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len)
{
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom((uint8_t)address, (uint8_t)len);
  uint8_t i = 0;
  while (Wire.available()) {
    data[i++] = Wire.read();
  }
  return 0;
}

uint16_t Watchy::WriteRegister(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len)
{
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.write(data, len);
  return (0 != Wire.endTransmission());
}

void Watchy::BmaConfig()
{
  if (m_sensor.begin(ReadRegister, WriteRegister, delay) == false) {
    // fail to init BMA
    return;
  }

  // Accel parameter structure
  Acfg cfg;
  /*!
      Output data rate in Hz, Optional parameters:
          - BMA4_OUTPUT_DATA_RATE_0_78HZ
          - BMA4_OUTPUT_DATA_RATE_1_56HZ
          - BMA4_OUTPUT_DATA_RATE_3_12HZ
          - BMA4_OUTPUT_DATA_RATE_6_25HZ
          - BMA4_OUTPUT_DATA_RATE_12_5HZ
          - BMA4_OUTPUT_DATA_RATE_25HZ
          - BMA4_OUTPUT_DATA_RATE_50HZ
          - BMA4_OUTPUT_DATA_RATE_100HZ
          - BMA4_OUTPUT_DATA_RATE_200HZ
          - BMA4_OUTPUT_DATA_RATE_400HZ
          - BMA4_OUTPUT_DATA_RATE_800HZ
          - BMA4_OUTPUT_DATA_RATE_1600HZ
  */
  cfg.odr = BMA4_OUTPUT_DATA_RATE_100HZ;
  /*!
      G-range, Optional parameters:
          - BMA4_ACCEL_RANGE_2G
          - BMA4_ACCEL_RANGE_4G
          - BMA4_ACCEL_RANGE_8G
          - BMA4_ACCEL_RANGE_16G
  */
  cfg.range = BMA4_ACCEL_RANGE_2G;
  /*!
      Bandwidth parameter, determines filter configuration, Optional parameters:
          - BMA4_ACCEL_OSR4_AVG1
          - BMA4_ACCEL_OSR2_AVG2
          - BMA4_ACCEL_NORMAL_AVG4
          - BMA4_ACCEL_CIC_AVG8
          - BMA4_ACCEL_RES_AVG16
          - BMA4_ACCEL_RES_AVG32
          - BMA4_ACCEL_RES_AVG64
          - BMA4_ACCEL_RES_AVG128
  */
  cfg.bandwidth = BMA4_ACCEL_NORMAL_AVG4;

  /*! Filter performance mode , Optional parameters:
      - BMA4_CIC_AVG_MODE
      - BMA4_CONTINUOUS_MODE
  */
  cfg.perf_mode = BMA4_CONTINUOUS_MODE;

  // Configure the BMA423 accelerometer
  m_sensor.setAccelConfig(cfg);

  // Enable BMA423 accelerometer
  // Warning : Need to use feature, you must first enable the accelerometer
  m_sensor.enableAccel();

  struct bma4_int_pin_config config;
  config.edge_ctrl = BMA4_LEVEL_TRIGGER;
  config.lvl       = BMA4_ACTIVE_HIGH;
  config.od        = BMA4_PUSH_PULL;
  config.output_en = BMA4_OUTPUT_ENABLE;
  config.input_en  = BMA4_INPUT_DISABLE;
  // The correct trigger interrupt needs to be configured as needed
  m_sensor.setINTPinConfig(config, BMA4_INTR1_MAP);

  struct bma423_axes_remap remap_data;
  remap_data.x_axis      = 1;
  remap_data.x_axis_sign = 0xFF;
  remap_data.y_axis      = 0;
  remap_data.y_axis_sign = 0xFF;
  remap_data.z_axis      = 2;
  remap_data.z_axis_sign = 0xFF;
  // Need to raise the wrist function, need to set the correct axis
  m_sensor.setRemapAxes(&remap_data);

  // Enable BMA423 isStepCounter feature
  m_sensor.enableFeature(BMA423_STEP_CNTR, true);
  // Enable BMA423 isTilt feature
  m_sensor.enableFeature(BMA423_TILT, true);
  // Enable BMA423 isDoubleClick feature
  m_sensor.enableFeature(BMA423_WAKEUP, true);

  // Reset steps
  //m_sensor.resetStepCounter();

  // Turn on feature interrupt
  //m_sensor.enableStepCountInterrupt();

  if (WAKE_ON_ACCEL_EVENTS) {
    m_sensor.enableTiltInterrupt();
    m_sensor.enableWakeupInterrupt();
  }
}