/**
 * Lynx.h - Lynxmotion 5 Robotic Arm Interface
 * CSE 490I Neurobotics Winter 2007
 * Josh Storz <jstorz@cs.washington.edu>
 */

#include <stdio.h>
#include <conio.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <windows.h>

#ifndef _LYNX_H_
#define _LYNX_H_

class Lynx
{
  public:
    Lynx();
    ~Lynx();
    
    // joint indexes
    static const int JOINT_BASE = 0;
    static const int JOINT_SHOULDER = 1;
    static const int JOINT_ELBOW = 2;
    static const int JOINT_WRIST = 3;
    static const int JOINT_GRIP = 4;

    // input indexes
    static const int INPUT_VA = 0;
    static const int INPUT_VB = 1;
    static const int INPUT_VC = 2;
    static const int INPUT_VD = 3;

    // lynx motor command
    struct LynxCmd
    {
      double joint_tgt[5];  // in radians, 0 to pi
      double joint_spd[5];  // in rad/s, negative # is as fast as possible, 0 is stop moving
      bool joint_set[5];    // true: set joint based on tgt and spd; false: ignore joint

      // default constructor
      LynxCmd()
      {
        for (int i = 0; i < 5; i++)
        {
          joint_tgt[i] = 0.0;
          joint_spd[i] = 0.0;
          joint_set[i] = false;
        }
      }
    };

    // lynx position reading
    struct LynxPos
    {
      double joint_pos[5];  // in radians, 0 to pi

      LynxPos()
      {
        for (int i = 0; i < 5; i++)
          joint_pos[i] = 0.0;
      }
    };

    // lynx input reading
    struct LynxInput
    {
      double analog_v[4];  // VA-D, in voltage: 0-4.98vdc

      LynxInput()
      {
        for (int i = 0; i < 4; i++)
          analog_v[i] = 0.0;
      }
    };

    // connection methods
    bool open();
    bool close();
    bool isConnected() { return (hcomm != NULL); }

    // settings
    void setComm(int port);
    int getComm();
    void setBaud(int rate);
    int getBaud();

    // control
    bool setCmd(Lynx::LynxCmd cmd);
    Lynx::LynxCmd getCmd() { return cmd_cur; }
    Lynx::LynxPos getPos() { updatePos(); return pos_cur; }
    Lynx::LynxInput getInput() { updateInput(); return in_cur; }

  protected:

    // pulse width constants
    static const int PULSE_MIN = 500;
    static const int PULSE_MAX = 2500;
    static const double DEGS_PER_PULSE;  //  0.09
    static const double RADS_PER_PULSE;  // ~0.0015708

    // instance variables
    HANDLE hcomm;               // comm handle
    int comm_port;              // comm port #
    int comm_baud;              // comm port baud rate
    LynxCmd cmd_cur;            // current (running) motor command
    LynxPos pos_cur;            // current joint positions (as of last update)
    LynxInput in_cur;           // current input values (as of last update)
    int pw_cur[5];              // current pulse width (as of last update)

    // helper methods
    int radsToPulse(double rad) { return max(PULSE_MIN, min(PULSE_MAX, (int)(rad / RADS_PER_PULSE) + PULSE_MIN)); }
    double pulseToRads(int pw) { return ((double)(pw - PULSE_MIN) * RADS_PER_PULSE); }
    bool sendMoveCmd(int num, int *ch, int *pw, int *spd);
    bool updatePos();    // read "real" positions off of controller
    bool updateInput();  // read input values

};

#endif