///////////////////////////////////////////////////////////////////////////
//  Copyright (C) Wizardry and Steamworks 2024 - License: MIT            //
//  Please see: http://www.gnu.org/licenses/gpl.html for legal details,  //
//  rights of fair usage, the disclaimer and warranty conditions.        //
///////////////////////////////////////////////////////////////////////////

#include <Bluepad32.h>
// ptz camera caddy control
#include "esp32-hal-ledc.h"
// wifi settings
#include <WiFi.h>
#include <ESPmDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>

// comment out to remove serial port debugging
//#define DEBUG_CONTROLLER_BUTTONS

///////////////////////////////////////////////////////////////////////////
// wireless and OTA settings                                             //
///////////////////////////////////////////////////////////////////////////

// the wifi network to connect to
#define WIFI_SSID ""
// the wifi password in plaintext
#define WIFI_PASSWORD ""
// the OTA password as an MD5 hash
#define OTA_PASSWORD_HASH ""

///////////////////////////////////////////////////////////////////////////
// controller button definitions                                         //
///////////////////////////////////////////////////////////////////////////
// these can be determined by enabling DEBUG, connecting a gamepad and   //
// then watching the console whilst pressing keys on the gamepad         //
///////////////////////////////////////////////////////////////////////////
#define CONTROLLER_BUTTON_A 2
#define CONTROLLER_BUTTON_B 1
#define CONTROLLER_BUTTON_Y 4
#define CONTROLLER_BUTTON_X 8

#define CONTROLLER_DPAD_LEFT 8
#define CONTROLLER_DPAD_RIGHT 4
#define CONTROLLER_DPAD_UP 1
#define CONTROLLER_DPAD_DOWN 2

///////////////////////////////////////////////////////////////////////////
// servo motor definitions for the PTZ caddy                             //
//     https://grimore.org/hardware/creating_a_ptz_caddy                 //
///////////////////////////////////////////////////////////////////////////
#define SERVO_CAMERA_PAN_GPIO 17
#define SERVO_CAMERA_PAN_LOW 1638
#define SERVO_CAMERA_PAN_HIGH 7864
#define SERVO_CAMERA_PAN_TIMER_WIDTH 16
#define SERVO_CAMERA_PAN_CHANNEL 11
#define SERVO_CAMERA_PAN_STEP 100
#define SERVO_CAMERA_PAN_FREQUENCY 50

#define SERVO_CAMERA_TILT_GPIO 5
#define SERVO_CAMERA_TILT_LOW 1638
#define SERVO_CAMERA_TILT_HIGH 7864
#define SERVO_CAMERA_TILT_TIMER_WIDTH 16
#define SERVO_CAMERA_TILT_CHANNEL 12
#define SERVO_CAMERA_TILT_STEP 100
#define SERVO_CAMERA_TILT_FREQUENCY 50

///////////////////////////////////////////////////////////////////////////
// miscellaneous                                                         //
///////////////////////////////////////////////////////////////////////////

#define HEADLIGHTS_GPIO 32

///////////////////////////////////////////////////////////////////////////
// declarations                                                          //
///////////////////////////////////////////////////////////////////////////

volatile int cameraPtzPan = (int)((SERVO_CAMERA_PAN_LOW + SERVO_CAMERA_PAN_HIGH) / 2.0),
             cameraPtzTilt = (int)((SERVO_CAMERA_TILT_LOW + SERVO_CAMERA_TILT_HIGH) / 2.0),
             retainPtzPan, retainPtzTilt;

ControllerPtr bleController;

TaskHandle_t task0, task1;
void coreTask_0(void * parameter);
void coreTask_1(void * parameter);

SemaphoreHandle_t mutexPan = xSemaphoreCreateMutex();
SemaphoreHandle_t mutexTilt = xSemaphoreCreateMutex();
volatile bool headlights = false;

///////////////////////////////////////////////////////////////////////////
// BLE connect and disconnect methods                                    //
///////////////////////////////////////////////////////////////////////////
void onConnectedController(ControllerPtr ctl) {
  ControllerProperties properties = ctl->getProperties();
  Serial.printf("Controller connected model: %s, VID=0x%04x, PID=0x%04x\n",
                ctl->getModelName().c_str(),
                properties.vendor_id,
                properties.product_id
               );
  bleController = ctl;

  // create two tasks scheduled on core 0 that is not used by bluepad32
  // that wil handle the pan and tilt of the PTZ caddy for the camera
  xTaskCreatePinnedToCore(
    coreTask_0,
    "pan",
    10000,
    NULL,
    1,
    &task0,
    0
  );

  xTaskCreatePinnedToCore(
    coreTask_1,
    "tilt",
    10000,
    NULL,
    1,
    &task1,
    0
  );
}

void onDisconnectedController(ControllerPtr ctl) {
  Serial.printf("Lost controller connection.");
  bleController = nullptr;

  // delete the camera PTZ control tasks
  vTaskDelete(task0);
  vTaskDelete(task1);
  BP32.forgetBluetoothKeys();
}

///////////////////////////////////////////////////////////////////////////
// camnera controls for the PTZ caddy                                    //
///////////////////////////////////////////////////////////////////////////
void cameraPanLeft() {
  Serial.printf("Current pan angle is %d.\n", cameraPtzPan);
  if (cameraPtzPan + SERVO_CAMERA_PAN_STEP > SERVO_CAMERA_PAN_HIGH) {
    return;
  }

  cameraPtzPan += SERVO_CAMERA_PAN_STEP;
}

void cameraPanRight() {
  Serial.printf("Current pan angle is %d.\n", cameraPtzPan);
  if (cameraPtzPan - SERVO_CAMERA_PAN_STEP < SERVO_CAMERA_PAN_LOW) {
    return;
  }

  cameraPtzPan -= SERVO_CAMERA_PAN_STEP;
}

void cameraTiltUp() {
  Serial.printf("Current tilt angle is %d.\n", cameraPtzTilt);
  if (cameraPtzTilt + SERVO_CAMERA_TILT_STEP > SERVO_CAMERA_TILT_HIGH) {
    return;
  }

  cameraPtzTilt += SERVO_CAMERA_TILT_STEP;
}

void cameraTiltDown() {
  Serial.printf("Current tilt angle is %d.\n", cameraPtzTilt);
  if (cameraPtzTilt - SERVO_CAMERA_TILT_STEP < SERVO_CAMERA_TILT_LOW) {
    return;
  }

  cameraPtzTilt -= SERVO_CAMERA_TILT_STEP;
}

///////////////////////////////////////////////////////////////////////////
// mecanum engine control                                                //
///////////////////////////////////////////////////////////////////////////
void engineStop() {
  // rf
  digitalWrite(22, LOW);
  digitalWrite(19, LOW);

  // rb
  digitalWrite(23, LOW);
  digitalWrite(18, LOW);

  // lf
  digitalWrite(16, LOW);
  digitalWrite(4, LOW);

  // lb
  digitalWrite(0, LOW);
  digitalWrite(2, LOW);
}

void nudgeEngineForward() {
  // rf
  digitalWrite(22, LOW);
  digitalWrite(19, HIGH);

  // rb
  digitalWrite(23, LOW);
  digitalWrite(18, HIGH);

  // lf
  digitalWrite(16, HIGH);
  digitalWrite(4, LOW);

  // lb
  digitalWrite(0, HIGH);
  digitalWrite(2, LOW);
}

void nudgeEngineForwardLeft() {
  // rf
  digitalWrite(22, LOW);
  digitalWrite(19, LOW);

  // rb
  digitalWrite(23, LOW);
  digitalWrite(18, HIGH);

  // lf
  digitalWrite(16, HIGH);
  digitalWrite(4, LOW);

  // lb
  digitalWrite(0, LOW);
  digitalWrite(2, LOW);
}

void nudgeEngineForwardRight() {
  // rf
  digitalWrite(22, LOW);
  digitalWrite(19, HIGH);

  // rb
  digitalWrite(23, LOW);
  digitalWrite(18, LOW);

  // lf
  digitalWrite(16, LOW);
  digitalWrite(4, LOW);

  // lb
  digitalWrite(0, HIGH);
  digitalWrite(2, LOW);
}

void nudgeEngineBackward() {
  // rf
  digitalWrite(22, HIGH);
  digitalWrite(19, LOW);

  // rb
  digitalWrite(23, HIGH);
  digitalWrite(18, LOW);

  // lf
  digitalWrite(16, LOW);
  digitalWrite(4, HIGH);

  // lb
  digitalWrite(0, LOW);
  digitalWrite(2, HIGH);
}

void nudgeEngineBackwardRight() {
  // rf
  digitalWrite(22, LOW);
  digitalWrite(19, LOW);

  // rb
  digitalWrite(23, HIGH);
  digitalWrite(18, LOW);

  // lf
  digitalWrite(16, LOW);
  digitalWrite(4, HIGH);

  // lb
  digitalWrite(0, LOW);
  digitalWrite(2, LOW);
}

void nudgeEngineBackwardLeft() {
  // rf
  digitalWrite(22, HIGH);
  digitalWrite(19, LOW);

  // rb
  digitalWrite(23, LOW);
  digitalWrite(18, LOW);

  // lf
  digitalWrite(16, LOW);
  digitalWrite(4, LOW);

  // lb
  digitalWrite(0, LOW);
  digitalWrite(2, HIGH);
}

void nudgeEngineTurnLeft() {
  // rf
  digitalWrite(22, LOW);
  digitalWrite(19, HIGH);

  // rb
  digitalWrite(23, LOW);
  digitalWrite(18, HIGH);

  // lf
  digitalWrite(16, LOW);
  digitalWrite(4, HIGH);

  // lb
  digitalWrite(0, LOW);
  digitalWrite(2, HIGH);
}

void nudgeEngineTurnRight() {
  // rf
  digitalWrite(22, HIGH);
  digitalWrite(19, LOW);

  // rb
  digitalWrite(23, HIGH);
  digitalWrite(18, LOW);

  // lf
  digitalWrite(16, HIGH);
  digitalWrite(4, LOW);

  // lb
  digitalWrite(0, HIGH);
  digitalWrite(2, LOW);
}

void nudgeEngineStrafeLeft() {
  // rf
  digitalWrite(22, HIGH);
  digitalWrite(19, LOW);

  // rb
  digitalWrite(23, LOW);
  digitalWrite(18, HIGH);

  // lf
  digitalWrite(16, HIGH);
  digitalWrite(4, LOW);

  // lb
  digitalWrite(0, LOW);
  digitalWrite(2, HIGH);
}

void nudgeEngineStrafeRight() {
  // rf
  digitalWrite(22, LOW);
  digitalWrite(19, HIGH);

  // rb
  digitalWrite(23, HIGH);
  digitalWrite(18, LOW);

  // lf
  digitalWrite(16, LOW);
  digitalWrite(4, HIGH);

  // lb
  digitalWrite(0, HIGH);
  digitalWrite(2, LOW);
}

///////////////////////////////////////////////////////////////////////////
// process controller actions                                            //
//                                                                       //
// combos implemented:                                                   //
//   * up - forward, down - back, left - turn left, right - turn right   //
//   * hold A + left, up or down, move the camera PTZ caddy              //
//   * hold B + left or right, strafe left and strafe right              //
///////////////////////////////////////////////////////////////////////////
void actControls(ControllerPtr ctrl) {
  switch (ctrl->dpad()) {
    case CONTROLLER_DPAD_UP:
      switch (ctrl->buttons()) {
        case CONTROLLER_BUTTON_A:
          Serial.println("Nudging camera up...");
          cameraTiltUp();
          return;
      }
      // go forward
      nudgeEngineForward();
      return;
    case CONTROLLER_DPAD_UP | CONTROLLER_DPAD_LEFT:
      nudgeEngineForwardLeft();
      return;
    case CONTROLLER_DPAD_DOWN:
      switch (ctrl->buttons()) {
        case CONTROLLER_BUTTON_A:
          Serial.println("Nudging camera down...");
          cameraTiltDown();
          return;
      }
      //go back
      nudgeEngineBackward();
      return;
    case CONTROLLER_DPAD_LEFT:
      switch (ctrl->buttons()) {
        case CONTROLLER_BUTTON_A:
          Serial.println("Nudging camera left...");
          cameraPanLeft();
          return;
        case CONTROLLER_BUTTON_X:
          nudgeEngineStrafeRight();
          return;
      }
      //turn left (implement strafe)
      nudgeEngineTurnLeft();
      return;
    case CONTROLLER_DPAD_RIGHT:
      switch (ctrl->buttons()) {
        case CONTROLLER_BUTTON_A:
          Serial.println("Nudging camera right...");
          cameraPanRight();
          return;
        case CONTROLLER_BUTTON_X:
          nudgeEngineStrafeLeft();
          return;
      }
      // turn right (implement strafe)
      nudgeEngineTurnRight();
      return;
  }

  switch(ctrl->buttons()) {
    case CONTROLLER_BUTTON_Y:
      switch(headlights) {
        case false:
          Serial.println("Turning headlights on...");
          digitalWrite(HEADLIGHTS_GPIO, LOW);
          break;
        case true:
        Serial.println("Turning headlights off...");
          digitalWrite(HEADLIGHTS_GPIO, HIGH);
          break;
      }
      headlights = !headlights;
      return;
  }

  engineStop();
}

///////////////////////////////////////////////////////////////////////////
// bluepad32 occupies core 1 such that all processing for the project    //
// will be carried out on core 0 for some separation of concerns         //
///////////////////////////////////////////////////////////////////////////
void coreTask_0(void * parameter) {
  ledcSetup(SERVO_CAMERA_PAN_CHANNEL, SERVO_CAMERA_PAN_FREQUENCY, SERVO_CAMERA_PAN_TIMER_WIDTH);
  ledcAttachPin(SERVO_CAMERA_PAN_GPIO, SERVO_CAMERA_PAN_CHANNEL);

  do {
    if (cameraPtzPan != retainPtzPan) {
      ledcWrite(SERVO_CAMERA_PAN_CHANNEL, cameraPtzPan);
      retainPtzPan = cameraPtzPan;
      delay(100);
      ledcWrite(SERVO_CAMERA_PAN_CHANNEL, 0);
    }

    vTaskDelay(1);
  } while (bleController && bleController->isConnected());
}

void coreTask_1(void * parameter) {
  ledcSetup(SERVO_CAMERA_TILT_CHANNEL, SERVO_CAMERA_TILT_FREQUENCY, SERVO_CAMERA_TILT_TIMER_WIDTH);
  ledcAttachPin(SERVO_CAMERA_TILT_GPIO, SERVO_CAMERA_TILT_CHANNEL);

  do {
    if (cameraPtzTilt != retainPtzTilt) {
      ledcWrite(SERVO_CAMERA_TILT_CHANNEL, cameraPtzTilt);
      retainPtzTilt = cameraPtzTilt;
      delay(100);
      ledcWrite(SERVO_CAMERA_TILT_CHANNEL, 0);
    }

    vTaskDelay(1);
  } while (bleController && bleController->isConnected());
}

///////////////////////////////////////////////////////////////////////////
//                            ARDUINO FUNCTIONS                          //
///////////////////////////////////////////////////////////////////////////
void setup() {
  Serial.begin(115200);
  // start wifi
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  while (WiFi.waitForConnectResult() != WL_CONNECTED) {
    Serial.println("Wifi connection failed! Rebooting...");
    delay(5000);
    ESP.restart();
  }
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());

  // setup OTA
  // Port defaults to 3232
  // ArduinoOTA.setPort(3232);

  // Hostname defaults to esp3232-[MAC]
  ArduinoOTA.setHostname("roverone");

  // No authentication by default
  //ArduinoOTA.setPassword(OTA_PASSWORD);

  // Password can be set with it's md5 value as well
  // MD5(admin) = 21232f297a57a5a743894a0e4a801fc3
  ArduinoOTA.setPasswordHash(OTA_PASSWORD_HASH);

  ArduinoOTA
  .onStart([]() {
    String type;
    if (ArduinoOTA.getCommand() == U_FLASH)
      type = "sketch";
    else // U_SPIFFS
      type = "filesystem";

    // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end()
    Serial.println("Start updating " + type);
  })
  .onEnd([]() {
    Serial.println("\nEnd");
    delay(5000);
    ESP.restart();
  })
  .onProgress([](unsigned int progress, unsigned int total) {
    Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
  })
  .onError([](ota_error_t error) {
    Serial.printf("Error[%u]: ", error);
    switch (error) {
      case OTA_AUTH_ERROR:
        Serial.println("Auth Failed");
        break;
      case OTA_BEGIN_ERROR:
        Serial.println("Begin Failed");
        break;
      case OTA_CONNECT_ERROR:
        Serial.println("Connect Failed");
        break;
      case OTA_RECEIVE_ERROR:
        Serial.println("Receive Failed");
        break;
      case OTA_END_ERROR:
        Serial.println("End Failed");
        break;
    }
  });

  ArduinoOTA.begin();
  Serial.println("OTA setup complete");

  // setup Bluepad32
  Serial.printf("Bluepad32 firmware version: %s\n", BP32.firmwareVersion());
  const uint8_t* addr = BP32.localBdAddress();
  Serial.printf("Bluetooth address: %2X:%2X:%2X:%2X:%2X:%2X\n",
                addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);

  // setup the Bluepad32 callbacks
  BP32.setup(&onConnectedController, &onDisconnectedController);

  // setup motors
  // rf
  pinMode(22, OUTPUT);
  pinMode(19, OUTPUT);

  // rb
  pinMode(23, OUTPUT);
  pinMode(18, OUTPUT);

  // lf
  pinMode(16, OUTPUT);
  pinMode(4, OUTPUT);

  // lb
  pinMode(0, OUTPUT);
  pinMode(2, OUTPUT);

  // set headlights to output
  pinMode(HEADLIGHTS_GPIO, OUTPUT);
}

// loop runs on core 1
void loop() {
  // handle any OTA updates
  ArduinoOTA.handle();
  // must be within loop()
  BP32.update();
  if (bleController && bleController->isConnected()) {
    // useful dump of all controls
#ifdef DEBUG_CONTROLLER_BUTTONS
    Serial.printf(
      "Gamepad control debug: "
      "idx=%d, dpad: 0x%02x, buttons: 0x%04x, axis L: %4d, %4d, axis R: %4d, %4d, brake: %4d, throttle: %4d, "
      "misc: 0x%02x, gyro x:%6d y:%6d z:%6d, accel x:%6d y:%6d z:%6d\n",
      bleController->index(),        // Controller Index
      bleController->dpad(),         // D-pad
      bleController->buttons(),      // bitmask of pressed buttons
      bleController->axisX(),        // (-511 - 512) left X Axis
      bleController->axisY(),        // (-511 - 512) left Y axis
      bleController->axisRX(),       // (-511 - 512) right X axis
      bleController->axisRY(),       // (-511 - 512) right Y axis
      bleController->brake(),        // (0 - 1023): brake button
      bleController->throttle(),     // (0 - 1023): throttle (AKA gas) button
      bleController->miscButtons(),  // bitmask of pressed "misc" buttons
      bleController->gyroX(),        // Gyro X
      bleController->gyroY(),        // Gyro Y
      bleController->gyroZ(),        // Gyro Z
      bleController->accelX(),       // Accelerometer X
      bleController->accelY(),       // Accelerometer Y
      bleController->accelZ()        // Accelerometer Z
    );
#endif
    // run the controller operations
    actControls(bleController);
  }

/*
  delay(5000);
  digitalWrite(32, HIGH);
  digitalWrite(LED_BUILTIN, HIGH);
  delay(5000);
  digitalWrite(32, LOW);
  digitalWrite(LED_BUILTIN, LOW);
*/
  delay(100);
}