/* --- pumainttest.C       Version 2.1
   --- test program for pumaInterface.c routines
   ---
   --- */

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <sys/perm.h>
#include <asm/io.h>

#include "../h/pumaInterface.h"

#define DEF_SPEED_PARAM         100.0
#define PUMA_NOMINAL_SPEED   (0.25) /* nominal speed (rads/sec) at DEF_SPEED_PARAM */

/* --- Global variables */
int             cntrlFlagG;
int             oscFlagG;


/* ******************************************************************** */
/*      Global variables                                                */
/* ******************************************************************** */

float   Q_cmdG[8];       /* first 6 are joint positions, checksum, type */
float   Q_deltaG[6];
int     mqStepnG;
float   mqStepSizeG;
float   moveqFreqG;
int     mqFlagG;
float   monSpeedG;

int print_menu()
{
  int    line =1;
  /* This switch is legacy, of course it's stupid */

  while (line > 0) {
  switch (line){
    case 1:
      printf("Menu options ->\n");
      break;
    case 2:
      printf("\ta\tRead an A/D voltage \n");
      break;
    case 3:
      printf("\tA\tRead all A/D voltages \n");
      break;
    case 4:
      if (cntrlFlagG){
        printf("\tc\tController toggle off \n");
      } else {
        printf("\tc\tController toggle on \n");
      } /* endif */
      break;
    case 5:
      printf("\tC\tCalibrate all joints\n");
      break;
    case 6:
      printf("\td\tDisable arm power \n");
      break;
    case 7:
      printf("\tD\tDiscrete output \n");
      break;
    case 8:
      printf("\te\tEnable arm power \n");
      break;
    case 9:
      printf("\tg\tDisplay gains \n");
      break;
    case 10:
      printf("\tG\tGenerate pot table for one joint \n");
      break;
    case 11:
      printf("\th\thand \n");
      break;
    case 12:
      printf("\tH\tHelp \n");
      break;
    case 13:
      printf("\tI\tIndex display \n");
      break;
    case 14:
      printf("\ti\tinput from any port \n");
      break;
    case 15:
      printf("\tJ\tJog any joint \n");
      break;
    case 16:
      printf("\tO\tOscillation \n");
      break;
    case 17:
      printf("\to\toutput to any port \n");
      break;
    case 18:
      printf("\tp\tPosition display \n");
      break;
    case 19:
      printf("\tP\tPosition reference display \n");
      break;
    case 20:
      printf("\tq\tQuit \n");
      break;
    case 21:
      printf("\tr\tReference position \n");
      break;
    case 22:
      printf("\ts\tStatus read \n");
      break;
    case 23:
      printf("\tT\tMove to next Trajectory point if defined \n");
      break;
    case 24:
      printf("\tw\tWrite a DAC voltage \n");
      break;
    case 25:
      printf("\tW\tWrite a DAC value in hex \n");
      break;
    case 26:
      printf("\tZ\tZero encoder counters \n");
      break;
    case 27:
      printf("\t?\tprint this list \n");
      break;
    default:
      line = -1;
      break;
  } /* endswitch */
  line++;
}
  return I_OK;
}

void print_help()
{
  printf("\nTo test proper wiring, DO NOT RUN THE CONTROLLER!\n");
  printf("Use the brake release and move the robot by hand to verify operation\n");
  printf("of the encoders, pots, and index registers.\n\n");
  printf("Connect a remote Emergency Power Off switch (EPO) before running controller.\n\n");
  printf("To run the controller:\n");
  printf("\tManually move the arm to the straight up position for safety.\n");
  printf("\tZ - to zero the encoders\n");
  printf("\te - to enable arm power\n");
  printf("\tc - to start the controller (check for green LED on TRC004)\n");
  printf("\tStay out of the workspace and keep the EPO in hand.\n");
  printf("\tPress the ARM POWER ON button. Arm should not move.\n");
  printf("\tPress O to make a joint oscillate. (O again halts joint)\n");
  printf("\tPress J to jog a joint. (<number> or <shift>-<number> moves +,-)\n");
  printf("\tPress T to move to the next trajectory point. (T again halts.)\n");
  printf("Some menu options only work with the controller off.\n");
  printf("\nA calibration file consists of a pot table for each joint concatenated\n");
  printf("together. Use 'G' to generate pot tables. (Turn controller off, press 'G',\n");
  printf("press 'e' (enable arm), press 'c' (controller), press ARM POWER ON button.)\n");
  printf("\nPress 'C' to spawn calibration. Turn on controller, enable ARM POWER, then\n");
  printf("press 'C' again to run calibration. When done, press 'C' to kill process.\n");
}


/* ******************************************************************** */
/*      MAIN                                                            */
/* ******************************************************************** */

int main (argc, argv)
    int argc;
    char *argv[];
{
  short         keepGoing = 1,
	        val[6],
		 ans, i, j;
		/* MS Change value to a short from int */
  short          jtNum,discrete;

  short         short_value;
  short         volts;
  int           hand=0;
  
  /* endif */

  if(TRC004_BASE < 0x3ff) {
    if (ioperm(TRC004_BASE, 64, 1)) {perror("ioperm"); exit(1);}
  } else 
    { iopl(3);
    }
  
  discrete=0;

  while(keepGoing){

    printf("Waiting for command > ");
    ans = getchar();
    switch (ans) {
    case 10:
      break;
    case 'a':       /* Read A/D */
      if (cntrlFlagG) break;
      printf("Enter joint number (1 - 6): ");
      scanf("%hd",&jtNum);
      jtNum--;
      if ((jtNum >= 0) && (jtNum <= 5)){
	short_value=pumaPotRd(jtNum, discrete);
	printf("A/D %d: %04X\n",jtNum+1,short_value);
      } else {
	printf("Joint number out of range \n");
      } /* endif */
      break;
    case 'A':       /* Read 6 A/D Values */
      for(jtNum=0;jtNum < 6;jtNum++){
	short_value=pumaPotRd(jtNum, discrete);
	printf("A/D %d: %04X\n",jtNum+1,short_value);
      }
      
      break;
    case 'c':       /* Control flag toggle */
      if (cntrlFlagG){
	printf("Controller off\n");
	discrete = discrete & ~POWER_BIT;
	pumaDesWr(discrete);
	cntrlFlagG = 0;
      } else {
	printf("Controller on\n");
	discrete = discrete | POWER_BIT;
	pumaDesWr(discrete);
	cntrlFlagG = 1;
      }  /* endif */
      break;
    case 'C':               /* Repeatedly read position*/
      for(i=0;i<500;i++) {
	for (j=0;j<6;j++) 
	  val[j]=pumaEncRd(j);
	printf("%6d  %6d  %6d \t%6d  %6d  %6d\n",val[0],
	       val[1], val[2],val[3],val[4], val[5]);
	sleep(1);
      }
      break;
    case 'd':       /* Disable arm power */
      discrete = discrete & ~POWER_BIT;
      pumaDesWr(discrete);
      break;
    case 'D':       /* Discretes */
      printf("Discretes are: %04X  enter new value in hex: ",
	     discrete);
      scanf("%hx",&discrete);
      pumaDesWr(discrete);
      break;
    case 'e':       /* Enable arm power */
      discrete = discrete | POWER_BIT;
      pumaDesWr(discrete);
      break;
    case 'g':
      break;
    case 'G':
      /*
      */
      break;
    case 'h':  /* Toggle sense of hand */
      if(hand) {
	discrete = discrete | HAND_OPEN_BIT;
	discrete = discrete & ~HAND_CLOSED_BIT;
	hand=0;
      } else {
	discrete = discrete | HAND_CLOSED_BIT;
	discrete = discrete & ~HAND_OPEN_BIT;
	hand=1;
      }
      pumaDesWr(discrete);
      break;
    case 'H':
      print_help();
      break;
    case 'i':
      break;
    case 'I':
      for (j=0;j<6;j++) 
	val[j]=pumaIndRd(j);
	printf("Indices: %6d  %6d  %6d \t%6d  %6d  %6d\n",val[0],
	       val[1], val[2],val[3],val[4], val[5]);
	
	break;
    case 'J':       /* Jog any joint by pressing the number keys */
      break;
    case 'o':       /* Output value to arbitrary port */
      break;
    case 'O':       /* oscillatory movement */
      break;
    case 'p':
      for (j=0;j<6;j++) 
	val[j]=pumaEncRd(j);
      printf("%6d  %6d  %6d \t%6d  %6d  %6d\n",val[0],
	     val[1], val[2],val[3],val[4], val[5]);
      
      break;
    case 'P':
      break;
    case 'q':       /* Quit */
      keepGoing = 0;
      break;
    case 'r':       /* Reference position */
      break;
    case 's':
      pumaStatus(STATUS,0,&short_value);
      printf("Status = %04x \n",short_value);
      break;
    case 't':
      break;
    case 'T':
      break;
    case 'w':       /* Write DAC value */
    case 'W':       /* Write DAC value */
      printf("Enter joint number (1 - 6): ");
      scanf("%hd",&jtNum);
      jtNum--;
      if ((jtNum >= 0) && (jtNum <= 5)){
	printf("Enter voltage in volts: ");
	scanf("%hd",&volts);
	pumaDacWr(volts,jtNum);
      } else {
	printf("Joint number out of range \n");
      } /* endif */
      break;

    case 'Z':       /* Zero the encoder counters */
      for(j=0;j<6;j++) pumaEncWr(0,j);
      break;
    case '?':       /* menu */
      print_menu();
      break;
    default:
      printf("Bad command -- '?' for command list\n");
    } /* endswitch */
    
  } /* endwhile */
  

  
  /* !!! IMPORTANT !!!
     --- Release shared memory and io port */
  if(TRC004_BASE < 0x3ff) {
   if (ioperm(TRC004_BASE, 64, 0)) {perror("ioperm"); exit(1);}
  }
  else iopl(0);

  exit();
}

