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

#include "Lynx.h"

const double Lynx::DEGS_PER_PULSE = 180.0 / (Lynx::PULSE_MAX - Lynx::PULSE_MIN);
const double Lynx::RADS_PER_PULSE =  M_PI / (Lynx::PULSE_MAX - Lynx::PULSE_MIN);

Lynx::Lynx()
{
  // initialize with default settings
  hcomm = NULL;
  comm_port = 1;
  comm_baud = 115200;

	armRange.min[0] = 0; armRange.max[0] = M_PI;
	armRange.min[1] = 0.8; armRange.max[1] = M_PI/2;
	armRange.min[2] = 0.8; armRange.max[2] = 2;
	armRange.min[3] = 0.8; armRange.max[3] = 2;
	armRange.min[4] = 0.8; armRange.max[4] = 2;
}

Lynx::~Lynx()
{
  // free comm port
  if (isConnected())
    close();
  if (hcomm != NULL)
    CloseHandle(hcomm);
}

bool Lynx::open()
{
  if (isConnected())
    return false;

  // open comm port
  
  char config_port[10];
  sprintf_s(config_port, 10, "\\\\.\\COM%d", comm_port);

  hcomm = CreateFile(config_port, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_ALWAYS, 0, NULL);

  if (hcomm == INVALID_HANDLE_VALUE)
  {
    hcomm = NULL;
    return false;
  }

  // configure communications port
  //   no parity, 8 bits, 1 stop bit, no flow control

  DCB dcb;
  memset(&dcb, 0, sizeof(dcb));
  dcb.DCBlength = sizeof(dcb);
  GetCommState(hcomm, &dcb);

  char config_str[50];
  sprintf_s(config_str, 50, "baud=%d parity=N data=8 stop=1", comm_baud);

  if (!SetupComm(hcomm, 128, 128) || !BuildCommDCB(config_str, &dcb))
  {
    CloseHandle(hcomm);
    hcomm = NULL;
    return false;
  }

  dcb.fOutxCtsFlow = FALSE;
  dcb.fOutxDsrFlow = FALSE;
  dcb.fDtrControl = DTR_CONTROL_DISABLE;
  dcb.fDtrControl = FALSE;
  dcb.fDsrSensitivity = FALSE;
  dcb.fOutX = FALSE;
  dcb.fInX = FALSE;
  dcb.fNull = FALSE;
  dcb.fRtsControl = RTS_CONTROL_DISABLE;
  dcb.fAbortOnError = FALSE;

  if (!SetCommState(hcomm ,&dcb))
  {
    CloseHandle(hcomm);
    hcomm = NULL;
    return false;
  }

  // set initial servo positions (sane start values)
  int num = 5;
  int ch[] = {0, 1, 2, 3, 4};
  int pw[] = {1500, 1500, 1500, 1500, 1500};
  int spd[] = {0, 0, 0, 0, 0};
  if (!sendMoveCmd(num, ch, pw, spd))
  {
    close();
    return false;
  }

  // wait for servos to move
  Sleep(200);
  
  // retrieve initial servo position (should be the same as set position above ^^^)
  if (!updatePos())
  {
    close();
    return false;
  }

  // retrieve initial inputs (also sets VA-D inputs to analog input no pullup)
  if (!updateInput())
  {
    close();
    return false;
  }

  return true;
}

bool Lynx::close()
{
  if (!isConnected())
    return false;

  CloseHandle(hcomm);
  hcomm = NULL;

  return true;
}

bool Lynx::demo()
{
	if (!isConnected())
	return false;
	
	Lynx::LynxCmd c;
	double tmp;
	int i;
	for( i=0; i<5; i++ ){
		c.joint_set[i] = true;
		c.joint_spd[i] = 1;
		c.joint_tgt[i] = (armRange.max[i]+armRange.min[i])/2;
	}
	this->setCmd(c); // set to the neutral position
	Sleep(1000);
	
	for( i=0; i<5; i++ )
		c.joint_set[i] = false;
		
	for( i=0; i<5; i++ ){
		printf( "Joint %d ... \n", i+1 );
		tmp = c.joint_tgt[i];
		c.joint_set[i] = true;
		c.joint_tgt[i] = armRange.min[i];
		this->setCmd(c); Sleep(1000);
		c.joint_tgt[i] = armRange.max[i];
		this->setCmd(c); Sleep(1000);
		c.joint_tgt[i] = tmp;
		this->setCmd(c); Sleep(1000);
		c.joint_set[i] = false;
	}
	printf( "demo over. welcome back!\n");
	return true;
}

Lynx::LynxRange Lynx::getRange()
{
	return this->armRange;
}

void Lynx::setRange( Lynx::LynxRange r )
{
	armRange = r;
}

void Lynx::setComm(int port)
{
  if (isConnected())
    return;
  
  comm_port = port;
}

int Lynx::getComm()
{
  return comm_port;
}

void Lynx::setBaud(int rate)
{
  if (isConnected())
    return;

  comm_baud = rate;
}

int Lynx::getBaud()
{
  return comm_baud;
}

bool Lynx::setCmd(Lynx::LynxCmd cmd)
{
  if (!isConnected())
    return false;

  int num = 0;
  int ch[5];
  int pw[5];
  int spd[5];

  updatePos();
  
  // merge with current command
  for (int i = 0; i < 5; i++)
  {
    if (cmd.joint_set[i])
    {
    	// set within the safe range
    	if (cmd.joint_tgt[i]< armRange.min[i])
    		cmd.joint_tgt[i] = armRange.min[i];
    	else if (cmd.joint_tgt[i] > armRange.max[i] )
    		cmd.joint_tgt[i] = armRange.max[i];
    		
      if (cmd.joint_spd[i] == 0)
      {
        // stop joint movement
        ch[num] = i;
        pw[num] = pw_cur[i];
        spd[num] = 0;
        cmd_cur.joint_set[i] = false;
        num++;
      }
      else if (cmd.joint_spd[i] < 0)
      {
        // as fast as possible
        ch[num] = i;
        pw[num] = radsToPulse(cmd.joint_tgt[i]);
        spd[num] = -1;
        cmd_cur.joint_set[i] = true;
        cmd_cur.joint_tgt[i] = cmd.joint_tgt[i];
        cmd_cur.joint_spd[i] = cmd.joint_spd[i];
        num++;
      }
      else
      {
        // at specified speed
        ch[num] = i;
        pw[num] = radsToPulse(cmd.joint_tgt[i]);
        spd[num] = radsToPulse(cmd.joint_spd[i]) - PULSE_MIN;
        cmd_cur.joint_set[i] = true;
        cmd_cur.joint_tgt[i] = cmd.joint_tgt[i];
        cmd_cur.joint_spd[i] = cmd.joint_spd[i];
        num++;
      }
    }
  }

  return sendMoveCmd(num, ch, pw, spd);
}

bool Lynx::sendMoveCmd(int num, int *ch, int *pw, int *spd)
{
  char buf[100];
  char cmdbuf[20];
  strcpy_s(buf, 100, "");

  // build movement command based on input
  for (int i = 0; i < num; i++)
  {
    if (spd[i] > 0)
      sprintf_s(cmdbuf, 20, "#%d P%d S%d ", ch[i], pw[i], spd[i]);
    else
      sprintf_s(cmdbuf, 20, "#%d P%d ", ch[i], pw[i], spd[i]);
    strcat_s(buf, 100, cmdbuf);
  }
  strcat_s(buf, 100, "\r");

  // send to controller
  DWORD written = 0;
  if (!WriteFile(hcomm, buf, (DWORD)strlen(buf), &written, NULL))
  {
    close();
    return false;
  }

  return true;
}

bool Lynx::updatePos()
{
  char readcmd[] = "QP 0 QP 1 QP 2 QP 3 QP 4\r";
  unsigned char buf[5];

  // request pulse widths
  DWORD written = 0;
  if (!WriteFile(hcomm, readcmd, (DWORD)strlen(readcmd), &written, NULL))
  {
    close();
    return false;
  }

  // read response
  DWORD read = 0;
  if (!ReadFile(hcomm, buf, 5, &read, NULL))
  {
    close();
    return false;
  }

  // update state
  for (int i = 0; i < 5; i++)
  {
    pw_cur[i] = (int)buf[i] * 10;
    pos_cur.joint_pos[i] = pulseToRads(pw_cur[i]);
  }

  return true;
}

bool Lynx::updateInput()
{
  char readcmd[] = "VA VB VC VD\r";
  unsigned char buf[4];

  // request analog input values
  DWORD written = 0;
  if (!WriteFile(hcomm, readcmd, (DWORD)strlen(readcmd), &written, NULL))
  {
    close();
    return false;
  }

  // read response
  DWORD read = 0;
  if (!ReadFile(hcomm, buf, 4, &read, NULL))
  {
    close();
    return false;
  }

  // update state
  for (int i = 0; i < 4; i++)
  {
    in_cur.analog_v[i] = (int)buf[i] * (5.0 / 256.0);
  }

  return true;
}

//// build-in tester method
//int main()
//{
//  // connect to lynx
//  Lynx l;
//  l.setComm(7);
//  printf("Attempting to connect to Lynx on COM %d... ", l.getComm());
//  if (!l.open())
//  {
//    printf("failed, aborting.\n\n");
//    return 1;
//  } else
//    printf("succeeded.\n\n");
//
//  // read in and print positions
//  printf("Reading in initial position values (rads):\n\n");
//
//  Lynx::LynxPos p = l.getPos();
//  printf("%4.5f  %4.5f  %4.5f  %4.5f  %4.5f\n\n", p.joint_pos[0], p.joint_pos[1], p.joint_pos[2], p.joint_pos[3], p.joint_pos[4]);
//
//  printf("Manual joint movement...\nWERTY to increase SDFGH to decrease q quit:\n\n");
//
//  // loop to adjust and display positions
//  Lynx::LynxCmd c;
//  for (int i = 0; i < 5; i++)
//  {
//    c.joint_set[i] = true;
//    c.joint_tgt[i] = p.joint_pos[i];
//    c.joint_spd[i] = M_PI / 8;
//  }
//  char ch = 0;
//  printf("Base        Shoulder    Elbow       Wrist       Grip\n");
//  do
//  {
//    printf("\r                                                                           ");
//
//    // simple control
//    if (ch == 'w')
//      c.joint_tgt[0] += M_PI / 32;
//    else if (ch == 's')
//      c.joint_tgt[0] -= M_PI / 32;
//    else if (ch == 'e')
//      c.joint_tgt[1] += M_PI / 32;
//    else if (ch == 'd')
//      c.joint_tgt[1] -= M_PI / 32;
//    else if (ch == 'r')
//      c.joint_tgt[2] += M_PI / 32;
//    else if (ch == 'f')
//      c.joint_tgt[2] -= M_PI / 32;
//    else if (ch == 't')
//      c.joint_tgt[3] += M_PI / 32;
//    else if (ch == 'g')
//      c.joint_tgt[3] -= M_PI / 32;
//    else if (ch == 'y')
//      c.joint_tgt[4] += M_PI / 32;
//    else if (ch == 'h')
//      c.joint_tgt[4] -= M_PI / 32;
//
//    // sync servos if key pressed
//    if (ch != 0 || ch != 'q')
//      l.setCmd(c);
//
//    ch = 0;
//    if (_kbhit())
//      ch = _getch();
//
//    // printing "real" position, not desired
//    p = l.getPos();
//    printf("\r%-10.5f  %-10.5f  %-10.5f  %-10.5f  %-10.5f", p.joint_pos[0], p.joint_pos[1], p.joint_pos[2], p.joint_pos[3], p.joint_pos[4]);
//
//	Sleep(10);
//  } while (ch != 'q');
//
//  printf("\n\n  ...quitting.\n\n");
//
//  return 0;
//}