#include <ardrone_api.h>
#include <VP_Os/vp_os_print.h>
#include <ardrone_tool/Navdata/ardrone_academy_navdata.h>

#include "control.h"

C_RESULT open_control_device(void);
C_RESULT update_control_device(void);
C_RESULT close_control_device(void);

input_device_t control_device = {
  "Control",
  open_control_device,
  update_control_device,
  close_control_device
};

controller_info_t controller_state, _z = {};

// TODO: remove this in final; not required
// Get keyboard input; return -1 if no input
#include <termios.h>
#include <unistd.h>
int kbhit(void) {
  struct termios oldt, newt;
  int ch;

  tcgetattr(STDIN_FILENO, &oldt);
  newt = oldt;

  newt.c_lflag &= ~( ICANON | ECHO );
  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
  ch = getchar();
  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);

  return ch;
}

C_RESULT open_control_device(void) {
  controller_state = _z;
  return C_OK;
}

C_RESULT update_control_device(void) {
  int key = kbhit();

  if (key == '1') {
    controller_state.start = !controller_state.start;
    ardrone_tool_set_ui_pad_start(controller_state.start);
    //ardrone_academy_navdata_takeoff();
  }
  else if (key == '2') {
    controller_state.emergency = !controller_state.emergency;
    ardrone_tool_set_ui_pad_select(controller_state.emergency);
    //ardrone_academy_navdata_emergency();
  }
  else {
    #define _INC(_c, _v) case _c : controller_state. _v += 0.5f; if ( controller_state. _v >  1000000.0f) controller_state. _v =  1.0f; break
    #define _DEC(_c, _v) case _c : controller_state. _v -= 0.5f; if ( controller_state. _v < -1000000.0f) controller_state. _v = -1.0f; break
    switch (key) {
      _INC('q', phi);
      _INC('w', theta);
      _INC('e', gaz);
      _INC('r', yaw);
      _INC('t', magneto_psi);
      _INC('y', magneto_psi_accuracy);

      _DEC('a', phi);
      _DEC('s', theta);
      _DEC('d', gaz);
      _DEC('f', yaw);
      _DEC('g', magneto_psi);
      _DEC('h', magneto_psi_accuracy);

      case '3': controller_state.hover_mode = !controller_state.hover_mode; break;
      case '4': controller_state.yaw_mode = !controller_state.yaw_mode; break;
      case '5': controller_state.absolute_mode = !controller_state.absolute_mode; break;
      default: key = -1;
    }
    #undef _INC
    #undef _DEC
      int32_t flags = (controller_state.hover_mode ? 1 : 0) | (controller_state.yaw_mode ? 2 : 0) | (controller_state.absolute_mode ? 4 : 0);
      ardrone_tool_set_progressive_cmd(
        flags, 
        controller_state.phi, // roll
        controller_state.theta, // pitch
        controller_state.gaz,
        controller_state.yaw,
        controller_state.magneto_psi, 
        controller_state.magneto_psi_accuracy
      );
  }
  
  return C_OK;
}

C_RESULT close_control_device(void) {
  return C_OK;
}
