#include "DXLMotion.hpp"
DXLMotion motion;

#define BAUDRATE (1000000)

// List of IDs for control target (Up to 32 axes)
const std::vector<uint8_t> my_id_list {1,2,3,4,5,6,7,8};

// Operation mode and drive mode for each ID
const DXLMotion::TServoInfo servo_info = { {1,4,0x4}, {2,4,0x4}, {3,4,0x4}, {4,4,0x4}, {5,4,0x4}, {6,4,0x4}, {7,4,0x4}, {8,4,0x4} };

// Motion data
const DXLMotion::TMotion motion0 = {
  my_id_list,
  // Angle/Velocity and transition time for each axis
  { { { 0, 0, 0, 0, 0, 0, 0, 0 }, 1.0 } }
};

const DXLMotion::TMotion motion1 = {
  my_id_list,
  { { { 90, 90, 90, 90, 90, 90, 90, 90 }, 1.0 } }
};

const DXLMotion::TMotion motion2 = {
  my_id_list,
  { { { -90, -90, -90, -90, -90, -90, -90, -90 }, 1.0 } }
};

const DXLMotion::TMotion motion3 = {
  my_id_list,
  { { { 180, 180, 180, 180, 180, 180, 180, 180 }, 1.0 } }
};

const DXLMotion::TMotion motion4 = {
  my_id_list,
  { { { -180, -180, -180, -180, -180, -180, -180, -180 }, 1.0 } }
};

const DXLMotion::TMotion motion5 = {
  my_id_list,
  {{ { -30, -30, -30, -30, -30, -30, -30, -30 }, 0.2 },
    { { 30, 30, 30, 30, 30, 30, 30, 30 }, 0.2 },
    { { -30, -30, -30, -30, -30, -30, -30, -30 }, 0.2 },
    { { 30, 30, 30, 30, 30, 30, 30, 30 }, 0.2 },
    { { -30, -30, -30, -30, -30, -30, -30, -30 }, 0.2 },
    { { 30, 30, 30, 30, 30, 30, 30, 30 }, 0.2 },
    { { -30, -30, -30, -30, -30, -30, -30, -30 }, 0.2 },
    { { 30, 30, 30, 30, 30, 30, 30, 30 }, 0.2 }
  }
};

const DXLMotion::TMotion motion6 = {
  my_id_list,
  { { { -45, 45, -45, 45, -45, 45, -45, 45 }, 2.0 },
    { { 90, 90, 90, 90, 90, 90, 90, 90 }, 2.0 },
    { { -30, -30, -30, -30, -30, -30, -30, -30 }, 2.0 },
    { { -180, -180, -180, -180, -180, -180, -180, -180 }, 2.0 }
  }
};

const DXLMotion::TMotion motion7 = {
  my_id_list,
  { { { 0, 0, 0, 0, 0, 0, 0, 0 }, 1.0 } }
};

const DXLMotion::TMotion motion8 = {
  my_id_list,
  { { { 0, 0, 0, 0, 0, 0, 0, 0 }, 1.0 } }
};

const DXLMotion::TMotion motion9 = {
  my_id_list,
  { { { 0, 0, 0, 0, 0, 0, 0, 0 }, 1.0 } }
};

const std::vector<const DXLMotion::TMotion *> mlist {
  &motion0, &motion1, &motion2, &motion3, &motion4, &motion5, &motion6, &motion7, &motion8, &motion9
};

// Alternative to the printf func
int Printf(const char *format, ...) {
  static char buf[200];
  va_list args;
  va_start(args, format);
  int result = vsnprintf(buf, sizeof(buf), format, args);
  va_end(args);
  Serial.print(buf);
  Serial.flush();
  return result;
}

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);

  Serial.begin(115200);
  Serial.ignoreFlowControl();

  motion.begin(BAUDRATE);
}

void loop() {
  char c;

  while (Serial.available()) {
    static bool led = false;
    digitalWrite(LED_BUILTIN, led ? HIGH : LOW);
    led = !led;

    switch (c = Serial.read()) {
      case '\33': Printf("\x1b[2J"); break;

      case '#':
        Serial.end(); delay(200); watchdog_enable(1, 1); while (1); break;
      case '!':
        Serial.end(); delay(200); reset_usb_boot(0, 0); break;

      // The first thing to do is to scan and initialize the servos.
      case 'i':
        Printf("init servos\n");
        motion.init_servo(&servo_info);
        motion.dx2lib->PrintDevicesList(&Printf);
        motion.set_gate(&my_id_list, true);
        Printf("fin\n");
        break;

      // Turn on servo control
      case 'e':
        Printf("enable ctrl\n");
        motion.set_gate(&my_id_list, true);
        break;
      // Turn off servo control
      case 'd':
        Printf("disable ctrl\n");
        motion.set_gate(&my_id_list, false);
        break;

      // Monitor current angle
      case 'm':
        Printf("get angles %d %d\n",my_id_list.data()[0], my_id_list.size());
        while (!Serial.available()) {
          double p[my_id_list.size()];
          motion.dx2lib->GetPresentAngles (my_id_list.data(), p, my_id_list.size());
          for (int i=0;i<my_id_list.size();i++) Printf("[%d]%6.1f ", my_id_list[i], p[i]);
          Printf("\r");
          delay(10);
        }
        Serial.read();
        Printf("\n");
        break;

      // Play Motion Data
      case '0' ... '9':
        Printf("play motion %c\n", c);
        motion.play(mlist[c - '0']);
        break;

      // Stops motion during playback
      case ' ':
        Printf("stop motion\n");
        motion.stop(&my_id_list);
        break;
    }
    static bool mode = false;
    digitalWrite(LED_BUILTIN, mode ? HIGH : LOW);
    mode = !mode;
  }
  __WFI();  // wait for Interrupt
}