#include "qt_pi.h"

#define _TASK_SLEEP_ON_IDLE_RUN
#define _TASK_INLINE
#define _TASK_TIMEOUT

#include <TaskScheduler.h>

Task *batt_mon;
Task *diff_control;
Task *move_cmd_timeout;
Task *odom_out;
Task *imu_out;
Task *ser_comm_timeout;

Scheduler ts;

void setup() {

  init_comm();

  init_enc();

  initMotorController();

  initBatt();

  resetPID();

  batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true,
                      NULL, NULL);
  diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true,
                          NULL, NULL);
  move_cmd_timeout =
      new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, false, NULL, NULL);
  odom_out =
      new Task(ODOM_INTERVAL, TASK_FOREVER, &sendOdom, &ts, false, NULL, NULL);
  imu_out = new Task(IMU_INTERVAL, TASK_FOREVER, &sendIMU, &ts, false, &initIMU,
                     NULL);
  ser_comm_timeout =
      new Task(COMM_RESET_INTERVAL, 1, &serCommTimeout, &ts, false, NULL, NULL);
  
}

void loop() {
  ts.execute();
  proc_serial();
}

void ser_comm_timeout_reset(){
     ser_comm_timeout->restartDelayed(COMM_RESET_INTERVAL);
}

void move_cmd_timeout_reset() {
     move_cmd_timeout->restartDelayed(AUTO_STOP_INTERVAL);
}

void enable_odom(bool a) {
  switch (a) {
  case 0:
    switch (odom_out->isEnabled()) {
    case 1:
      odom_out->disable();
      break;
    }
    break;

  case 1:
    switch (odom_out->isEnabled()) {
    case 0:
      odom_out->enable();
      break;
    }
    break;
  }
}

void enable_imu(bool a) {
  switch (a) {
  case 0:
    switch (imu_out->isEnabled()) {
    case 1:
      imu_out->disable();
      break;
    }
    break;

  case 1:
    switch (imu_out->isEnabled()) {
    case 0:
      imu_out->enable();
      break;
    }
    break;
  }
}
