/***************************************************************
 * PrDriverTest.cpp
 *
 * This is a program to test drivers.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 02/25/03: Weyn Ong  : Added JR3 accel sensor.
 * 02/24/03: Weyn Ong  : Added JR3 force sensor.
 * 04/20/98: K.C. Chang: added zebra sensor.
 * 11/02/97: K.C. Chang: adapted to the new PrRobot class.
 * 10/26/97: K.C. Chang: created.
 */

#include "PrGlobalDefn.h"
#include <iostream.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "PrDACTRC.h"
#include "PrADCTRC.h"
#include "PrTimer.h"
#include "PrEncoderTRC.h"
#include "PrEncoderPC7166.h"
#include "PrITimer.h"
#include "PrIPBoardIntf.h"
#include "PrIPBoardTRC.h"
#include "PrForceSensorATI.h"
#include "PrForceSensorZebra.h"
#include "PrForceSensorJr3ISA.h"
#include "PrForceSensorJr3PCI.h"
#include "PrAccelSensorJr3PCI.h"

#define OK 1;
#define ERROR 0;

int dacTest(int argc,char **argv);
int adcTest(int argc,char **argv);
int timerTest(int argc,char **argv);
int encoderTest(int argc,char **argv);
int encoder2Test(int argc,char **argv);
int itimerTest(int argc,char **argv);
int ipBoardTest(int argc,char **argv);
int servoTest(int argc,char **argv);
int fsTest(int argc,char **argv);
int zebraTest(int argc,char **argv);
int jr3ISATest(int argc,char **argv);
int jr3PCITest(int argc,char **argv);
int jr3PCIAccelTest(int argc,char **argv);

int main(int argc,char **argv) 
{ 
    if (argc < 2) 
      {
	  cout << "[dac|adc|timer|encoder|ipBoard|fs|zebra|jr3isa|jr3pci" <<
            "|accel|servo] [args]" <<endl;
	  return ERROR;
      }

    char *prog = argv[1];

    if (strcmp(prog,"dac") == 0)
      dacTest(argc,argv);
    else if (strcmp(prog,"adc") == 0)
      adcTest(argc,argv);
    else if (strcmp(prog,"timer") == 0)
      timerTest(argc,argv);
    else if (strcmp(prog,"encoder") == 0)
      encoderTest(argc,argv);
    else if (strcmp(prog,"encoder2") == 0)
      encoder2Test(argc,argv);
    else if (strcmp(prog,"itimer") == 0)
      itimerTest(argc,argv);
    else if (strcmp(prog,"ipBoard") == 0)
      ipBoardTest(argc,argv);
    else if (strcmp(prog,"servo") == 0)
      servoTest(argc,argv);
    else if (strcmp(prog,"fs") == 0)
      fsTest(argc,argv);
    else if (strcmp(prog,"zebra") == 0)
      zebraTest(argc,argv);
    else if (strcmp(prog,"jr3isa") == 0)
      jr3ISATest(argc,argv);
    else if (strcmp(prog,"jr3pci") == 0)
      jr3PCITest(argc,argv);
    else if (strcmp(prog,"accel") == 0)
      jr3PCIAccelTest(argc,argv);
    else
      cout<<"error: unknown test: "<<prog<<endl;

    return OK;
}

int dacTest(int argc,char **argv)
{
    if (argc < 4) 
      {
	  cout << "dac [channel] [volt]"<<endl;
	  return ERROR;
      }

    int channel = atoi(argv[2]);
    Float volt = atof(argv[3]);

    PrDACTRC *dac = new PrDACTRC;
//    dac->selfTest();
    dac->value(channel,volt);
    delete dac;

    return OK;
}


int adcTest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "adc [channel]"<<endl;
	  return ERROR;
      }

    int channel = atoi(argv[2]);
    Float volt = 0.0;

    int discreteReg = 0x0000;
    
    PrADCTRC *adc = new PrADCTRC(discreteReg);
//    adc->selfTest();
    adc->value(channel,volt);
    delete adc;
    return OK;
}

int timerTest(int argc,char **argv) 
{ 
    if (argc < 4) 
      {
	  cout << "timer [frequency] [loop]"<<endl;
	  return ERROR;
      }
    
    int frequency = atoi(argv[2]);
    int loop = atoi(argv[3]);
    
    PrTimer *timer = new PrTimer(frequency);
    timer->selfTest(loop);
    delete timer;
    return OK;
}

int encoderTest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "encoder [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    
    PrEncoderTRC *encoder = new PrEncoderTRC;
    PrTimer *timer = new PrTimer(100);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  Float values[6];
	  encoder->values(values);
	  printf("%d: %f %f %f %f %f %f\n",i,values[0],values[1],values[2],
		 values[3],values[4],values[5]);
	  timer->wait();
      } 
    delete timer;
    delete encoder;
    return OK;
}

int encoder2Test(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "encoder2 [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    PrEncoderPC7166 *encoder = new PrEncoderPC7166;
    PrTimer *timer = new PrTimer(5);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  Float values[2];
	  encoder->values(values);
	  printf("%d: %f %f\n",i,values[0],values[1]);
	  timer->wait();
      } 
    delete timer;
    delete encoder;
    return OK;
}

int itimerTest(int argc,char **argv) 
{ 
    if (argc < 4) 
      {
	  cout << "itimer [frequency] [loop]"<<endl;
	  return ERROR;
      }
    
    int frequency = atoi(argv[2]);
    int loop = atoi(argv[3]);
    
    PrITimer *itimer = new PrITimer(frequency);
    itimer->selfTest(loop);
    delete itimer;
    return OK;
}

int ipBoardTest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "ipBoard [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    
    PrIPBoardIntf *hwInterface = new PrIPBoardTRC(100,PR_ARM_ID,
						   PR_GRIPPER_ID,
						   PR_FS_ID,
                                                   PR_ACCEL_ID,
						   PR_BASE_ID);

    PrTimer *timer = new PrTimer(100);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  Float values[6];
	  hwInterface->jointPosition(values);
//	  hwInterface->gripperPosition(&values[7]);
	  printf("%d: %f %f %f %f %f %f\n",i,
		 values[0],values[1],values[2],
		 values[3],values[4],values[5]);
	  timer->wait();
      } 
    //(PrIPBoardTRC)hwInterface->handOpen();

    delete timer;
    delete hwInterface;
    return OK;
}

int fsTest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "fs [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    Float q[6];

    PrForceSensorATI *fs = new PrForceSensorATI;
    PrTimer *timer = new PrTimer(1000);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  fs->values(q);
	  printf("%d: %f %f %f %f %f %f\n",i,q[0],q[1],q[2],q[3],q[4],q[5]);
	  timer->wait();
      } 
    delete timer;
    delete fs;
    return OK;
}

int zebraTest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "zebra [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    Float q[6];

    PrForceSensorZebra *fs = new PrForceSensorZebra;
    PrTimer *timer = new PrTimer(1000);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  fs->values(q);
	  printf("%d: %f %f %f %f %f %f\n",i,q[0],q[1],q[2],q[3],q[4],q[5]);
	  timer->wait();
      } 
    delete timer;
    delete fs;
    return OK;
}

int jr3ISATest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "jr3isa [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    Float q[6];

    PrForceSensorJr3 * fs = new PrForceSensorJr3ISA();
    if ( fs->flag() != PR_DRIVER_OK )
        return -1;
    PrTimer *timer = new PrTimer(100);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  fs->values(q);
	  printf("%d: %f %f %f %f %f %f\n",i,q[0],q[1],q[2],q[3],q[4],q[5]);
	  timer->wait();
      } 
    delete timer;
    delete fs;
    return OK;
}

int jr3PCITest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "jr3pci [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    Float q[6];

    PrForceSensorJr3 * fs = new PrForceSensorJr3PCI();
    if ( fs->flag() != PR_DRIVER_OK )
        return -1;
    PrTimer *timer = new PrTimer(100);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  fs->values(q);
	  printf("%d: %f %f %f %f %f %f\n",i,q[0],q[1],q[2],q[3],q[4],q[5]);
	  timer->wait();
      } 
    printf( "Error count : %d\n", fs->getErrorCount() );
    delete timer;
    delete fs;
    return OK;
}

int jr3PCIAccelTest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "accel [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    Float q[6];

    PrAccelSensorIntf * fs = new PrAccelSensorJr3PCI();
    if ( fs->flag() != PR_DRIVER_OK )
        return -1;
    PrTimer *timer = new PrTimer(100);
    timer->start();
    for(int i=0;i<loop;i++)
      {
	  fs->values(q);
	  printf("%d: %f %f %f %f %f %f\n",i,q[0],q[1],q[2],q[3],q[4],q[5]);
	  timer->wait();
      } 
    delete timer;
    delete fs;
    return OK;
}

int servoTest(int argc,char **argv) 
{ 
    if (argc < 3) 
      {
	  cout << "servo [loop]"<<endl;
	  return ERROR;
      }
    
    int loop = atoi(argv[2]);
    
    Float Kp[7] = {2500.0, 1600.0, 1600.0, 400.0, 100.0, 100.0, 100.0};
    Float Kv[7] = {100.0, 80.0, 80.0, 40.0, 20.0, 20.0, 20.0};
    Float qd[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    Float q[7];
    Float dq[7];
    Float tau[7];

    PrIPBoardIntf *hwInterface = new PrIPBoardTRC(1000,PR_ARM_ID,
						   PR_GRIPPER_ID,
						   PR_FS_ID,
                                                   PR_ACCEL_ID,
						   PR_BASE_ID);
    PrTimer *timer = new PrTimer(1000);
    timer->start();
    hwInterface->powerOn();
    hwInterface->jointPosition(qd);
//	printf("qd: %f %f %f %f %f %f %f\n",qd[0],qd[1],qd[2],qd[3],qd[4],qd[5],qd[6]); 
//	loop=0;
    for(int i=0;i<loop;i++)
      {
	  hwInterface->jointPosition(q);
	  hwInterface->jointVelocity(q,dq);
//	  printf("%d: %f %f %f %f %f %f %f\n",i,
//  	  q[0],q[1],q[2],q[3],q[4],q[5],q[6]);
	  for(int j=0;j<7;j++)
		{
	    tau[j] = -Kp[j]*(q[j]-qd[j])-Kv[j]*dq[j]; 
		}
//	printf("tau: %f %f %f %f %f %f %f\n",
//	tau[0],tau[1],tau[2],tau[3],tau[4],tau[5],tau[6]);	
	  hwInterface->jointForce(tau);
	  timer->wait();
      } 
    hwInterface->powerOff();
    delete timer;
    delete hwInterface;
    return OK;
}

