#include <FreeRTOS.h>
#include <timers.h>
#include <semphr.h>

#include "DXLMotion.hpp"
//#include <dx2memmap.h>
#include "src/dxlib/dx2memmap.h"

#define _RS485_TX (4)
#define _RS485_RX (5)

uint32_t DXLMotion::us_init(uint32_t baud) {
  Serial2.setRX(_RS485_RX);
  Serial2.setTX(_RS485_TX);
  Serial2.setFIFOSize(512);
  Serial2.begin(baud, UART_PARITY_NONE | SERIAL_STOP_BIT_1);
  Serial2.setTimeout(10);
  actualbaud = uart_set_baudrate(uart1, baud);
  return actualbaud;
}
void DXLMotion::us_deinit(void) {
  Serial2.end();
}
uint32_t DXLMotion::us_setbaudrate(uint32_t baud) {
  us_deinit();
  return us_init(baud);
}
void DXLMotion::us_rxpurge(void) {
  while (Serial2.available()) Serial2.read();
}
void DXLMotion::us_putc(uint8_t c) {
  Serial2.write(c);
}
void DXLMotion::us_puts(const uint8_t *buf, int len) {
  Serial2.write(buf, len);
}
int DXLMotion::us_gets(uint8_t *buf, int len) {
  if (len == 0) return 0;
  return Serial2.readBytes(buf, len);
}
void DXLMotion::us_flush(void) {
  Serial2.flush();
}

void DXLMotion::slock(void) {
  xSemaphoreTake(xMutex, portMAX_DELAY);
}
void DXLMotion::sfree(void) {
  xSemaphoreGive(xMutex);
}

void DXLMotion::MotionTimerCB(TimerHandle_t xTimer) {
  xTimerStop(MotionTimer, 0);
  if (++motionind < SpecMotion->motion.size()) {
    dx2lib->SetMultipleGoalValues(SpecMotion->ids.data(), &SpecMotion->motion[motionind].angles[0], SpecMotion->ids.size(), SpecMotion->motion[motionind].sec);
    xTimerChangePeriod(MotionTimer, pdMS_TO_TICKS(SpecMotion->motion[motionind].sec * 1000), portMAX_DELAY);
  }
}

void DXLMotion::begin(uint32_t baud) {
  if (!initialized) {
    anchor = this;
    dx2lib = new DX2LIB((DX2LIB::PDXHost_ConfParam)&param);
    dx2lib->begin(baud);
    xMutex = xSemaphoreCreateMutex();
    dx2lib->SemLock = _slock;
    dx2lib->SemFree = _sfree;
    MotionTimer = xTimerCreate("Timer", pdMS_TO_TICKS(1000), pdFALSE, anchor, _MotionTimerCB);
    xTimerStop(MotionTimer, 0);
    initialized = true;
  }
}

void DXLMotion::end(void) {
  if (!initialized) {
    //  vTaskDelete (task_mon_handler);
    xTimerDelete(MotionTimer, 0);
    dx2lib->end();
    dx2lib->SemLock = nullptr;
    dx2lib->SemFree = nullptr;
    delete dx2lib;
    initialized = false;
    anchor = nullptr;
  }
}

bool DXLMotion::play(const TMotion *motion) {
  if (!initialized) return false;
  //bool DXLMotion::play(const std::vector<uint8_t> *target_ids, const std::vector<TMotion> *motion) {
  if (detect_ids.size() == 0) return false;
  xTimerStop(MotionTimer, 0);
  motionind = 0;
  SpecMotion = (TMotion *)(void *)motion;
  if (dx2lib->SetMultipleGoalValues(SpecMotion->ids.data(), &SpecMotion->motion[motionind].angles[0], SpecMotion->ids.size(), SpecMotion->motion[motionind].sec)) {
    xTimerChangePeriod(MotionTimer, pdMS_TO_TICKS(SpecMotion->motion[motionind].sec * 1000), portMAX_DELAY);
    return (xTimerStart(MotionTimer, portMAX_DELAY) == pdPASS);
  }
  return false;
}

bool DXLMotion::stop(const std::vector<uint8_t> *target_ids) {
  if (!initialized) return false;
  if (detect_ids.size() == 0) return false;
  xTimerStop(MotionTimer, 0);
  motionind = 0;
  return dx2lib->StandStillAngles(target_ids->data(), target_ids->size());
}

bool DXLMotion::init_servo(const TServoInfo *sinfo) {
  if (!initialized) return false;
  detect_ids.clear();
  dx2lib->InitDevicesList();

  Serial2.setTimeout(10);
  for (const auto &info : *sinfo)
    if (dx2lib->GetModelInfo(info.id)->devtype != devtNONE) detect_ids.push_back(info.id);
  bool result = (detect_ids.size() == sinfo->size());
  Serial2.setTimeout(50);
  if (detect_ids.size() > 0) {
    for (const auto &info : *sinfo) {
      if (dx2lib->Reboot(info.id)) {
        // When dynamically switching modes of operation, we don't want previously set volatile parameters to remain, so we encourage a reboot.
        for (int i=0;i<20 && !dx2lib->Ping(info.id);i++) ;
        if (dx2lib->SetOperatingMode(info.id, info.op_mode)) {
          if (dx2lib->SetDriveMode(info.id, info.drive_mode)) {
          } else result = false;
        } else result = false;
      } else result = false;
    }
  } else result = false;
  return result;
}

bool DXLMotion::set_gate(const std::vector<uint8_t> *target_ids, bool en) {
  if (!initialized) return false;
  return dx2lib->SetTorqueEnablesEquival(target_ids->data(), target_ids->size(), en);
}

bool DXLMotion::get_angle(const std::vector<uint8_t> *target_ids, std::vector<double> *pangle) {
  if (!initialized) return false;
  return dx2lib->GetPresentAngles(target_ids->data(), pangle->data(), target_ids->size());
}

bool DXLMotion::get_velocity(const std::vector<uint8_t> *target_ids, std::vector<double> *pvelo) {
  if (!initialized) return false;
  return dx2lib->GetPresentVelocities(target_ids->data(), pvelo->data(), target_ids->size());
}

bool DXLMotion::get_current(const std::vector<uint8_t> *target_ids, std::vector<double> *pcur) {
  if (!initialized) return false;
  return dx2lib->GetPresentCurrents(target_ids->data(), pcur->data(), target_ids->size());
}

DXLMotion *DXLMotion::anchor = nullptr;