/*
 * Copyright 2023 jacqueline <me@jacqueline.id.au>, robin <robin@rhoward.id.au>
 *
 * SPDX-License-Identifier: GPL-3.0-only
 */

#include "drivers/haptics.hpp"
#include <stdint.h>

#include <cstdint>
#include <initializer_list>
#include <mutex>
#include <variant>

#include "assert.h"
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "drivers/nvs.hpp"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/projdefs.h"
#include "hal/gpio_types.h"
#include "hal/i2c_types.h"

#include "drivers/i2c.hpp"

namespace drivers {

static constexpr char kTag[] = "haptics";
static constexpr uint8_t kHapticsAddress = 0x5A;

Haptics::Haptics(NvsStorage& nvs) {
  // Bring the driver out of standby, and put it into calibration mode.
  WriteRegister(Register::kMode, 0b00000111);

  if (nvs.HapticMotorIsErm()) {
    ESP_LOGI(kTag, "Setting up ERM motor...");

    // Put into ERM Open Loop:
    // (§8.5.4.1 Programming for ERM Open-Loop Operation)

    // - Turn off N_ERM_LRA first
    WriteRegister(Register::kFeedbackControl,
                  static_cast<uint8_t>(RegisterDefaults::kFeedbackControl) &
                      (~ControlMask::kNErmLra));

    // - Turn on ERM_OPEN_LOOP
    WriteRegister(Register::kControl3,
                  static_cast<uint8_t>(RegisterDefaults::kControl3) |
                      ControlMask::kErmOpenLoop);

    // Set library
    // TODO(robin): try the other libraries and test response. C is marginal, D
    // too much?
    WriteRegister(Register::kWaveformLibrary,
                  static_cast<uint8_t>(kDefaultErmLibrary));

  } else {
    ESP_LOGI(kTag, "Setting up LRA motor...");

    // Set rated voltage to 1v8; see equation (5) in Section 8.5.2.1.
    WriteRegister(Register::kRatedVoltage, 0x46);

    // Set overdrive voltage to 2v6 see equation (9) in Section 8.5.2.2. This
    // is based on a max operating voltage of 1v85 RMS.
    // FIXME: Could this be higher?
    WriteRegister(Register::kOverdriveClampVoltage, 0x7B);

    // Set the drive time for our 235Hz motors. This is a period of 4.2ms, so
    // a drive time of 2.1ms. See Table 24 in section 8.6.21. Leave the other
    // bits in this register with their default values.
    WriteRegister(Register::kControl1, 0b10010000);

    // Ensure closed-loop mode is set.
    WriteRegister(Register::kControl3, 0b10000000);

    auto calibration = nvs.LraCalibration();
    if (!calibration) {
      ESP_LOGI(kTag, "calibrating LRA motor");
      // Enable LRA mode, with default brake factor, loop gain, and back-EMF.
      WriteRegister(Register::kFeedbackControl, 0b10110110);

      // Start auto-calibration.
      WriteRegister(Register::kGo, 1);
      // Wait for calibration to complete.
      do {
        vTaskDelay(1);
      } while ((ReadRegister(Register::kGo) & 1) > 0);

      uint8_t status = ReadRegister(Register::kStatus);
      if ((status & 0b11111) == 0) {
        calibration = NvsStorage::LraData{
            .compensation =
                ReadRegister(Register::kAutoCalibrationCompensationResult),
            .back_emf = ReadRegister(Register::kAutoCalibrationBackEmfResult),
            .gain = static_cast<uint8_t>(
                ReadRegister(Register::kFeedbackControl) & 0b11),
        };

        ESP_LOGI(kTag, "lra calibration succeeded: %x%x%x",
                 calibration->compensation, calibration->back_emf,
                 calibration->gain);

        nvs.LraCalibration(*calibration);
      } else {
        // Not much we can do here! The motor might still buzz okay, so it's
        // not a fatal issue at least.
        ESP_LOGE(kTag, "lra calibration failed :(");
      }
    } else {
      ESP_LOGI(kTag, "using stored lra calibration: %x%x%x",
               calibration->compensation, calibration->back_emf,
               calibration->gain);

      WriteRegister(Register::kAutoCalibrationCompensationResult,
                    calibration->compensation);
      WriteRegister(Register::kAutoCalibrationBackEmfResult,
                    calibration->back_emf);
      WriteRegister(Register::kFeedbackControl, 0b10110100 | calibration->gain);
    }

    // Set library; only option is the LRA one for, well, LRA motors.
    WriteRegister(Register::kWaveformLibrary,
                  static_cast<uint8_t>(Library::LRA));
  }

  // Set mode (internal trigger, on writing 1 to Go register)
  WriteRegister(Register::kMode, static_cast<uint8_t>(Mode::kInternalTrigger));

  // Set up a default effect (sequence of one effect)
  SetWaveformEffect(kStartupEffect);
}

Haptics::~Haptics() {}

void Haptics::WriteRegister(Register reg, uint8_t val) {
  uint8_t regRaw = static_cast<uint8_t>(reg);
  I2CTransaction transaction;
  transaction.start()
      .write_addr(kHapticsAddress, I2C_MASTER_WRITE)
      .write_ack(regRaw, val)
      .stop();
  esp_err_t res = transaction.Execute(1);
  if (res != ESP_OK) {
    ESP_LOGW(kTag, "write failed: %s", esp_err_to_name(res));
  }
}

auto Haptics::ReadRegister(Register reg) -> uint8_t {
  uint8_t regRaw = static_cast<uint8_t>(reg);
  uint8_t ret = 0;
  I2CTransaction transaction;
  transaction.start()
      .write_addr(kHapticsAddress, I2C_MASTER_WRITE)
      .write_ack(regRaw)
      .start()
      .write_addr(kHapticsAddress, I2C_MASTER_READ)
      .read(&ret, I2C_MASTER_NACK)
      .stop();
  esp_err_t res = transaction.Execute(1);
  if (res != ESP_OK) {
    ESP_LOGW(kTag, "read failed: %s", esp_err_to_name(res));
  }
  return ret;
}

auto Haptics::PlayWaveformEffect(Effect effect) -> void {
  const std::lock_guard<std::mutex> lock{playing_effect_};  // locks until freed

  // Interrupt any already-running effect. This makes haptic feedback feel a
  // little bit more responsive.
  WriteRegister(Register::kGo, 0);

  SetWaveformEffect(effect);
  Go();
}

// Starts the pre-programmed sequence
auto Haptics::Go() -> void {
  WriteRegister(Register::kGo,
                static_cast<uint8_t>(RegisterDefaults::kGo) | 0b00000001);
}

auto Haptics::SetWaveformEffect(Effect effect) -> void {
  if (!current_effect_ || current_effect_.value() != effect) {
    WriteRegister(Register::kWaveformSequenceSlot1,
                  static_cast<uint8_t>(effect));
    WriteRegister(Register::kWaveformSequenceSlot2,
                  static_cast<uint8_t>(Effect::kStop));
  }
  current_effect_ = effect;
}

auto Haptics::TourEffects() -> void {
  TourEffects(Effect::kFirst, Effect::kLast, kDefaultErmLibrary);
}
auto Haptics::TourEffects(Library lib) -> void {
  TourEffects(Effect::kFirst, Effect::kLast, lib);
}
auto Haptics::TourEffects(Effect from, Effect to) -> void {
  TourEffects(from, to, kDefaultErmLibrary);
}
auto Haptics::TourEffects(Effect from, Effect to, Library lib) -> void {
  ESP_LOGI(kTag, "With library #%u...", static_cast<uint8_t>(lib));

  for (uint8_t e = static_cast<uint8_t>(from);
       e <= static_cast<uint8_t>(to) &&
       e <= static_cast<uint8_t>(Effect::kLast);
       e++) {
    auto effect = static_cast<Effect>(e);
    auto label = EffectToLabel(effect);

    if (effect == Effect::kDontUseThis_Longbuzzforprogrammaticstopping_100Pct) {
      ESP_LOGI(kTag, "Ignoring effect '%s'...", label.c_str());
      continue;
    }

    ESP_LOGI(kTag, "Playing effect #%u: %s", e, label.c_str());
    PlayWaveformEffect(effect);
    Go();

    vTaskDelay(pdMS_TO_TICKS(800 /*ms*/));
  }
}

auto Haptics::TourLibraries(Effect from, Effect to) -> void {
  for (uint8_t lib = 1; lib <= 5; lib++) {
    WriteRegister(Register::kWaveformLibrary, lib);

    for (auto e = static_cast<uint8_t>(Effect::kFirst);
         e <= static_cast<uint8_t>(Effect::kLast); e++) {
      auto effect = static_cast<Effect>(e);
      ESP_LOGI(kTag, "Library %u, Effect: %s", lib,
               EffectToLabel(effect).c_str());

      PlayWaveformEffect(effect);
      Go();

      vTaskDelay(pdMS_TO_TICKS(800 /*ms*/));
    }
  }
}

auto Haptics::PowerDown() -> void {
  WriteRegister(Register::kMode, static_cast<uint8_t>(Mode::kInternalTrigger) |
                                     ModeMask::kStandby);
}

auto Haptics::Reset() -> void {
  WriteRegister(Register::kMode, static_cast<uint8_t>(Mode::kInternalTrigger) |
                                     ModeMask::kDevReset);
}

auto Haptics::PowerUp() -> void {
  // FIXME: technically overwriting the RESERVED bits of Mode, but eh
  uint8_t newMask = static_cast<uint8_t>(Mode::kInternalTrigger) &
                    (~ModeMask::kStandby) & (~ModeMask::kDevReset);
  WriteRegister(Register::kMode,
                static_cast<uint8_t>(RegisterDefaults::kMode) | newMask);
}

auto Haptics::EffectToLabel(Effect effect) -> std::string {
  switch (static_cast<Effect>(effect)) {
    case Effect::kStrongClick_100Pct:
      return "Strong Click - 100%";
    case Effect::kStrongClick_60Pct:
      return "Strong Click (60%)";
    case Effect::kStrongClick_30Pct:
      return "Strong Click (30%)";
    case Effect::kSharpClick_100Pct:
      return "Sharp Click (100%)";
    case Effect::kSharpClick_60Pct:
      return "Sharp Click (60%)";
    case Effect::kSharpClick_30Pct:
      return "Sharp Click (30%)";
    case Effect::kSoftBump_100Pct:
      return "Soft Bump (100%)";
    case Effect::kSoftBump_60Pct:
      return "Soft Bump (60%)";
    case Effect::kSoftBump_30Pct:
      return "Soft Bump (30%)";
    case Effect::kDoubleClick_100Pct:
      return "Double Click (100%)";
    case Effect::kDoubleClick_60Pct:
      return "Double Click (60%)";
    case Effect::kTripleClick_100Pct:
      return "Triple Click (100%)";
    case Effect::kSoftFuzz_60Pct:
      return "Soft Fuzz (60%)";
    case Effect::kStrongBuzz_100Pct:
      return "Strong Buzz (100%)";
    case Effect::k750msAlert_100Pct:
      return "750ms Alert (100%)";
    case Effect::k1000msAlert_100Pct:
      return "1000ms Alert (100%)";
    case Effect::kStrongClick1_100Pct:
      return "Strong Click1 (100%)";
    case Effect::kStrongClick2_80Pct:
      return "Strong Click2 (80%)";
    case Effect::kStrongClick3_60Pct:
      return "Strong Click3 (60%)";
    case Effect::kStrongClick4_30Pct:
      return "Strong Click4 (30%)";
    case Effect::kMediumClick1_100Pct:
      return "Medium Click1 (100%)";
    case Effect::kMediumClick2_80Pct:
      return "Medium Click2 (80%)";
    case Effect::kMediumClick3_60Pct:
      return "Medium Click3 (60%)";
    case Effect::kSharpTick1_100Pct:
      return "Sharp Tick1 (100%)";
    case Effect::kSharpTick2_80Pct:
      return "Sharp Tick2 (80%)";
    case Effect::kSharpTick3_60Pct:
      return "Sharp Tick3 (60%)";
    case Effect::kShortDoubleClickStrong1_100Pct:
      return "Short Double Click Strong1 (100%)";
    case Effect::kShortDoubleClickStrong2_80Pct:
      return "Short Double Click Strong2 (80%)";
    case Effect::kShortDoubleClickStrong3_60Pct:
      return "Short Double Click Strong3 (60%)";
    case Effect::kShortDoubleClickStrong4_30Pct:
      return "Short Double Click Strong4 (30%)";
    case Effect::kShortDoubleClickMedium1_100Pct:
      return "Short Double Click Medium1 (100%)";
    case Effect::kShortDoubleClickMedium2_80Pct:
      return "Short Double Click Medium2 (80%)";
    case Effect::kShortDoubleClickMedium3_60Pct:
      return "Short Double Click Medium3 (60%)";
    case Effect::kShortDoubleSharpTick1_100Pct:
      return "Short Double Sharp Tick1 (100%)";
    case Effect::kShortDoubleSharpTick2_80Pct:
      return "Short Double Sharp Tick2 (80%)";
    case Effect::kShortDoubleSharpTick3_60Pct:
      return "Short Double Sharp Tick3 (60%)";
    case Effect::kLongDoubleSharpClickStrong1_100Pct:
      return "Long Double Sharp Click Strong1 (100%)";
    case Effect::kLongDoubleSharpClickStrong2_80Pct:
      return "Long Double Sharp Click Strong2 (80%)";
    case Effect::kLongDoubleSharpClickStrong3_60Pct:
      return "Long Double Sharp Click Strong3 (60%)";
    case Effect::kLongDoubleSharpClickStrong4_30Pct:
      return "Long Double Sharp Click Strong4 (30%)";
    case Effect::kLongDoubleSharpClickMedium1_100Pct:
      return "Long Double Sharp Click Medium1 (100%)";
    case Effect::kLongDoubleSharpClickMedium2_80Pct:
      return "Long Double Sharp Click Medium2 (80%)";
    case Effect::kLongDoubleSharpClickMedium3_60Pct:
      return "Long Double Sharp Click Medium3 (60%)";
    case Effect::kLongDoubleSharpTick1_100Pct:
      return "Long Double Sharp Tick1 (100%)";
    case Effect::kLongDoubleSharpTick2_80Pct:
      return "Long Double Sharp Tick2 (80%)";
    case Effect::kLongDoubleSharpTick3_60Pct:
      return "Long Double Sharp Tick3 (60%)";
    case Effect::kBuzz1_100Pct:
      return "Buzz1 (100%)";
    case Effect::kBuzz2_80Pct:
      return "Buzz2 (80%)";
    case Effect::kBuzz3_60Pct:
      return "Buzz3 (60%)";
    case Effect::kBuzz4_40Pct:
      return "Buzz4 (40%)";
    case Effect::kBuzz5_20Pct:
      return "Buzz5 (20%)";
    case Effect::kPulsingStrong1_100Pct:
      return "Pulsing Strong1 (100%)";
    case Effect::kPulsingStrong2_60Pct:
      return "Pulsing Strong2 (60%)";
    case Effect::kPulsingMedium1_100Pct:
      return "Pulsing Medium1 (100%)";
    case Effect::kPulsingMedium2_60Pct:
      return "Pulsing Medium2 (60%)";
    case Effect::kPulsingSharp1_100Pct:
      return "Pulsing Sharp1 (100%)";
    case Effect::kPulsingSharp2_60Pct:
      return "Pulsing Sharp2 (60%)";
    case Effect::kTransitionClick1_100Pct:
      return "Transition Click1 (100%)";
    case Effect::kTransitionClick2_80Pct:
      return "Transition Click2 (80%)";
    case Effect::kTransitionClick3_60Pct:
      return "Transition Click3 (60%)";
    case Effect::kTransitionClick4_40Pct:
      return "Transition Click4 (40%)";
    case Effect::kTransitionClick5_20Pct:
      return "Transition Click5 (20%)";
    case Effect::kTransitionClick6_10Pct:
      return "Transition Click6 (10%)";
    case Effect::kTransitionHum1_100Pct:
      return "Transition Hum1 (100%)";
    case Effect::kTransitionHum2_80Pct:
      return "Transition Hum2 (80%)";
    case Effect::kTransitionHum3_60Pct:
      return "Transition Hum3 (60%)";
    case Effect::kTransitionHum4_40Pct:
      return "Transition Hum4 (40%)";
    case Effect::kTransitionHum5_20Pct:
      return "Transition Hum5 (20%)";
    case Effect::kTransitionHum6_10Pct:
      return "Transition Hum6 (10%)";

    // TODO: fix labels for XtoY-style
    case Effect::kTransitionRampDownLongSmooth1_100to0Pct:
      return "Transition Ramp Down Long Smooth1 (100→0%)";
    case Effect::kTransitionRampDownLongSmooth2_100to0Pct:
      return "Transition Ramp Down Long Smooth2 (100→0%)";
    case Effect::kTransitionRampDownMediumSmooth1_100to0Pct:
      return "Transition Ramp Down Medium Smooth1 (100→0%)";
    case Effect::kTransitionRampDownMediumSmooth2_100to0Pct:
      return "Transition Ramp Down Medium Smooth2 (100→0%)";
    case Effect::kTransitionRampDownShortSmooth1_100to0Pct:
      return "Transition Ramp Down Short Smooth1 (100→0%)";
    case Effect::kTransitionRampDownShortSmooth2_100to0Pct:
      return "Transition Ramp Down Short Smooth2 (100→0%)";
    case Effect::kTransitionRampDownLongSharp1_100to0Pct:
      return "Transition Ramp Down Long Sharp1 (100→0%)";
    case Effect::kTransitionRampDownLongSharp2_100to0Pct:
      return "Transition Ramp Down Long Sharp2 (100→0%)";
    case Effect::kTransitionRampDownMediumSharp1_100to0Pct:
      return "Transition Ramp Down Medium Sharp1 (100→0%)";
    case Effect::kTransitionRampDownMediumSharp2_100to0Pct:
      return "Transition Ramp Down Medium Sharp2 (100→0%)";
    case Effect::kTransitionRampDownShortSharp1_100to0Pct:
      return "Transition Ramp Down Short Sharp1 (100→0%)";
    case Effect::kTransitionRampDownShortSharp2_100to0Pct:
      return "Transition Ramp Down Short Sharp2 (100→0%)";
    case Effect::kTransitionRampUpLongSmooth1_0to100Pct:
      return "Transition Ramp Up Long Smooth1 (0→100%)";
    case Effect::kTransitionRampUpLongSmooth2_0to100Pct:
      return "Transition Ramp Up Long Smooth2 (0→100%)";
    case Effect::kTransitionRampUpMediumSmooth1_0to100Pct:
      return "Transition Ramp Up Medium Smooth1 (0→100%)";
    case Effect::kTransitionRampUpMediumSmooth2_0to100Pct:
      return "Transition Ramp Up Medium Smooth2 (0→100%)";
    case Effect::kTransitionRampUpShortSmooth1_0to100Pct:
      return "Transition Ramp Up Short Smooth1 (0→100%)";
    case Effect::kTransitionRampUpShortSmooth2_0to100Pct:
      return "Transition Ramp Up Short Smooth2 (0→100%)";
    case Effect::kTransitionRampUpLongSharp1_0to100Pct:
      return "Transition Ramp Up Long Sharp1 (0→100%)";
    case Effect::kTransitionRampUpLongSharp2_0to100Pct:
      return "Transition Ramp Up Long Sharp2 (0→100%)";
    case Effect::kTransitionRampUpMediumSharp1_0to100Pct:
      return "Transition Ramp Up Medium Sharp1 (0→100%)";
    case Effect::kTransitionRampUpMediumSharp2_0to100Pct:
      return "Transition Ramp Up Medium Sharp2 (0→100%)";
    case Effect::kTransitionRampUpShortSharp1_0to100Pct:
      return "Transition Ramp Up Short Sharp1 (0→100%)";
    case Effect::kTransitionRampUpShortSharp2_0to100Pct:
      return "Transition Ramp Up Short Sharp2 (0→100%)";
    case Effect::kTransitionRampDownLongSmooth1_50to0Pct:
      return "Transition Ramp Down Long Smooth1 (50→0%)";
    case Effect::kTransitionRampDownLongSmooth2_50to0Pct:
      return "Transition Ramp Down Long Smooth2 (50→0%)";
    case Effect::kTransitionRampDownMediumSmooth1_50to0Pct:
      return "Transition Ramp Down Medium Smooth1 (50→0%)";
    case Effect::kTransitionRampDownMediumSmooth2_50to0Pct:
      return "Transition Ramp Down Medium Smooth2 (50→0%)";
    case Effect::kTransitionRampDownShortSmooth1_50to0Pct:
      return "Transition Ramp Down Short Smooth1 (50→0%)";
    case Effect::kTransitionRampDownShortSmooth2_50to0Pct:
      return "Transition Ramp Down Short Smooth2 (50→0%)";
    case Effect::kTransitionRampDownLongSharp1_50to0Pct:
      return "Transition Ramp Down Long Sharp1 (50→0%)";
    case Effect::kTransitionRampDownLongSharp2_50to0Pct:
      return "Transition Ramp Down Long Sharp2 (50→0%)";
    case Effect::kTransitionRampDownMediumSharp1_50to0Pct:
      return "Transition Ramp Down Medium Sharp1 (50→0%)";
    case Effect::kTransitionRampDownMediumSharp2_50to0Pct:
      return "Transition Ramp Down Medium Sharp2 (50→0%)";
    case Effect::kTransitionRampDownShortSharp1_50to0Pct:
      return "Transition Ramp Down Short Sharp1 (50→0%)";
    case Effect::kTransitionRampDownShortSharp2_50to0Pct:
      return "Transition Ramp Down Short Sharp2 (50→0%)";
    case Effect::kTransitionRampUpLongSmooth_10to50Pct:
      return "Transition Ramp Up Long Smooth10to (10→50%)";
    case Effect::kTransitionRampUpLongSmooth_20to50Pct:
      return "Transition Ramp Up Long Smooth20to (10→50%)";
    case Effect::kTransitionRampUpMediumSmooth_10to50Pct:
      return "Transition Ramp Up Medium Smooth (10→50%)";
    case Effect::kTransitionRampUpMediumSmooth_20to50Pct:
      return "Transition Ramp Up Medium Smooth (20→50%)";
    case Effect::kTransitionRampUpShortSmooth_10to50Pct:
      return "Transition Ramp Up Short Smooth (10→50%)";
    case Effect::kTransitionRampUpShortSmooth_20to50Pct:
      return "Transition Ramp Up Short Smooth (20→50%)";
    case Effect::kTransitionRampUpLongSharp_10to50Pct:
      return "Transition Ramp Up Long Sharp (10→50%)";
    case Effect::kTransitionRampUpLongSharp_20to50Pct:
      return "Transition Ramp Up Long Sharp (20→50%)";
    case Effect::kTransitionRampUpMediumSharp_10to50Pct:
      return "Transition Ramp Up Medium Sharp (10→50%)";
    case Effect::kTransitionRampUpMediumSharp_20to50Pct:
      return "Transition Ramp Up Medium Sharp (20→50%)";
    case Effect::kTransitionRampUpShortSharp_10to50Pct:
      return "Transition Ramp Up Short Sharp (10→50%)";
    case Effect::kTransitionRampUpShortSharp_20to50Pct:
      return "Transition Ramp Up Short Sharp (20→50%)";
    case Effect::kDontUseThis_Longbuzzforprogrammaticstopping_100Pct:
      return "DON'T USE: Long Buzz for Programmatic Stopping (100%)";
    case Effect::kSmoothHum1NoKickOrBrakePulse_50Pct:
      return "Smooth Hum1 No Kick Or Brake Pulse (50%)";
    case Effect::kSmoothHum2NoKickOrBrakePulse_40Pct:
      return "Smooth Hum2 No Kick Or Brake Pulse (40%)";
    case Effect::kSmoothHum3NoKickOrBrakePulse_30Pct:
      return "Smooth Hum3 No Kick Or Brake Pulse (30%)";
    case Effect::kSmoothHum4NoKickOrBrakePulse_20Pct:
      return "Smooth Hum4 No Kick Or Brake Pulse (20%)";
    case Effect::kSmoothHum5NoKickOrBrakePulse_10Pct:
      return "Smooth Hum5 No Kick Or Brake Pulse (10%)";
    default:
      return "UNKNOWN EFFECT";
  }
}

}  // namespace drivers