/*
  Multiple DXLs are linked together to move like an animation.
  Use commands for physical values in DX2LIB.

  2023 T.Uemitu
*/

#pragma once

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

#include "Arduino.h"
//#include <dx2lib.h>
#include "src/dxlib/dx2lib.h"

#define MAX_AXISNUM 32

class DXLMotion {
public:
  typedef struct {
    uint8_t id;
    uint8_t op_mode;
    uint8_t drive_mode;
  } TTServoInfo;
  typedef std::vector<TTServoInfo> TServoInfo;

  typedef double TPose[MAX_AXISNUM];
  typedef struct {
    TPose angles;
    double sec;
  } TTMotion;
  typedef struct {
    const std::vector<uint8_t> ids;
    const std::vector<TTMotion> motion;
  } TMotion;

  uint32_t actualbaud;
  DX2LIB *dx2lib;

  void begin(uint32_t baud);
  void end(void);

  bool init_servo(const TServoInfo *sinfo);

  bool play(const TMotion *motion);
  bool stop(const std::vector<uint8_t> *target_ids);

  bool set_gate(const std::vector<uint8_t> *target_ids, bool en);
  bool get_angle(const std::vector<uint8_t> *target_ids, std::vector<double> *pangle);
  bool get_velocity(const std::vector<uint8_t> *target_ids, std::vector<double> *pvelo);
  bool get_current(const std::vector<uint8_t> *target_ids, std::vector<double> *pcur);

private:
  static DXLMotion *anchor;

  bool initialized = false;

  std::vector<uint8_t> detect_ids;
  // Motion data structure
  TMotion *SpecMotion;
  int motionind;
  SemaphoreHandle_t xMutex = nullptr;
  TimerHandle_t MotionTimer = nullptr;
  xTaskHandle task_mon_handler;

  uint32_t us_init(uint32_t baud);
  void us_deinit(void);
  uint32_t us_setbaudrate(uint32_t baud);
  void us_rxpurge(void);
  void us_putc(uint8_t c);
  void us_puts(const uint8_t *buf, int len);
  int us_gets(uint8_t *buf, int len);
  void us_flush(void);
  static uint32_t _us_init(uint32_t baud) { return anchor->us_init(baud); };
  static void _us_deinit(void) { anchor->us_deinit(); };
  static uint32_t _us_setbaudrate(uint32_t baud) { return anchor->us_setbaudrate(baud); };
  static void _us_rxpurge(void) { anchor->us_rxpurge(); };
  static void _us_putc(uint8_t c) { anchor->us_putc(c); };
  static void _us_puts(const uint8_t *buf, int len) { anchor->us_puts(buf, len); };
  static int _us_gets(uint8_t *buf, int len) { return anchor->us_gets(buf,len); };
  static void _us_flush(void) { anchor->us_flush(); };

  const DX2LIB::TDXHost_ConfParam param = { _us_init, _us_deinit, _us_setbaudrate, _us_rxpurge, _us_putc, _us_puts, _us_gets, _us_flush };

  void slock(void);
  void sfree(void);
  static void _slock(void) { anchor->slock(); };
  static void _sfree(void) { anchor->sfree(); };

  void MotionTimerCB(TimerHandle_t xTimer);
  static void _MotionTimerCB(TimerHandle_t xTimer) { anchor->MotionTimerCB(xTimer); }

  void task_mon();
};