#include "..\\Lynx.h"

int main()
{
  Lynx l;
  // connect to lynx: set the COM port
  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");

  printf( "Input the file name for recording:\n" );
  char fname[100];
  size_t namelen;
  _cgets_s( fname, 99, &namelen );
  fname[namelen] = '\0';
  FILE *fp = fopen( fname, "w" );
  if( !fp ){
	  printf( "Failed to open file to write. aborting.\n" );
	  return 1;
  }

  // read in and print positions
  printf("Reading in initial position values (rads):\n\n");

  Lynx::LynxPos p = l.getPos();
  fprintf(fp, "%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]);

  fclose( fp );
  printf("\n\n  ...quitting.\n\n");
  return 0;
}