/* --- calib.c
   ---
   --- Calibration routines for PUMA robots.
   --- See ROBOTUSR.C for examples of how they are used.
   ---
   --- 01/23/96 RMV     Initial Coding.
   --- 01/28/98 RMV	Changed robot_potrd() back to robot_longrd().
   ---
   --- */

#include <stdio.h>
#include <stdlib.h>
#include "vtypes.h"
#include "robot.h"
#include "sched.h"

#define TABLE(ptr,i,j)  ((ptr)[i*2+j])
#define MAX_JTS         6

short   magicNumG[MAX_JTS];
int     *potTableG[MAX_JTS];
int     calJointsG;
int     doneJointsG[MAX_JTS];
float   potcalRangeG[2];
int     potcalFlagG;
const float     calIncG[6] = {0.002, 0.001, 0.002, 0.004, 0.004, 0.005};

/* --- State machine that actually generates the pot table.
   --- Users do not call this function directly. (see potcal_joint().) */
int potcal()
{

  return I_OK;
}

/* --- Function that actually calibrates one or six joints.
   --- Users do not call this function directly. (see calib_init().) */
int calibrate()
{

  return I_OK;
}

/* --- This function prepares the robot for power-up calibration.
   --- The user calls this function first, then calls calib_joint(). */
processT *calib_init(char *calFile)
{
  int   jt, cnt, i, j, maxCnt;
  char  tmp[256];
  FILE  *fp;

  /* The calibration file contains lists of encoder values and pot values for each
     index pulse for each joint. */
  if ((fp = fopen(calFile, "rt")) == NULL){
    printf("ERROR: Calibration file %s not found \n",calFile);
    return NULL;
  } /* endif */

  /* NULL all pointers because some PUMAs have only 5 joints. NULL indicates which
     joints to skip. */
  for (i=0; i<MAX_JTS; i++)
    potTableG[i] = NULL;

  /* Read the file, one joint at a time. The list for each joint begins with the
     joint number (1-MAX_JTS) and a count of the number of table entries for that
     joint. */
  while(fscanf(fp,"%d %d",&jt, &cnt) != EOF){
    if (fgets(tmp, 256, fp) == NULL)
      break;

    printf("jt = %d, cnt = %d\n",jt,cnt);

    /* allocate space for the joint list */
    if ((potTableG[--jt] = (int *)malloc(2*(cnt+1)*sizeof(int))) == NULL){
      printf("Error: calib_init -- malloc failed \n");
      return NULL;
    } /* endif */

	 /* printf("pointer = %04X:%04X \n",FP_SEG(potTableG[jt]),FP_OFF(potTableG[jt]));
       */
    TABLE(potTableG[jt],0,0) = jt+1;
    TABLE(potTableG[jt],0,1) = cnt;
    for (i=1; i<=cnt; i++){
      fscanf(fp, "%d %d",&TABLE(potTableG[jt],i,0), &TABLE(potTableG[jt],i,1));
      fgets(tmp, 256, fp);
    } /* endfor */

  } /* endwhile */
  printf("Done with file %s\n",calFile);
  fclose(fp);

  for(j=0; j<MAX_JTS; j++){
    doneJointsG[j] = 0;
    if (potTableG[j] != NULL){
      maxCnt = TABLE(potTableG[j],0,1);
      printf("maxCnt = %d\n",maxCnt);
      for(i=0; i<=maxCnt; i++)
        printf("%d %d \n",TABLE(potTableG[j],i,0),TABLE(potTableG[j],i,1));
    } /* endif */
  } /* endfor */

  robot_control(CLR_INDEX_BITS, 0, NULL);

  /*   return sbsSpawn(calibrate, 100.0, 75);
*/
}

/* --- Calibrate a single joint or all joints sequentially.
   --- This is the function called by the user. */
int calib_joint(int jt, processT *calProc)
{
  calJointsG = jt;
  sbsControl(calProc, SBS_ON);

  return I_OK;
}

/* --- Generate a pot table for a single joint.
   --- This is the function called by the user. */
processT *potcal_joint()
{
  short         shortVal = 0;
  int           jt;
  processT      *calProc;

  /* Spawn the potcal process */
  /*  calProc = sbsSpawn(potcal, 100.0, 75);
*/

  /* power down the robot because the queries suspend the controller */
  robot_control(POWER,0,&shortVal);

  jt = 0;
  while ((jt < 1) || (jt > 6)){
    printf("Joint number (1-6): ");
    scanf("%d", &jt);
  } /* endwhile */

  printf("Maximum joint position (radians): ");
  scanf("%f",&potcalRangeG[0]);
  printf("Minimum joint position (radians): ");
  scanf("%f",&potcalRangeG[1]);

  calJointsG = jt;

  /* allocate space for the joint list */
  if ((potTableG[--jt] = (int *)malloc(512*sizeof(int))) == NULL){
    printf("Error: potcal_joint -- malloc failed \n");
    sbsControl(calProc, SBS_KILL);
    return NULL;
  } /* endif */

  printf("Make sure a controller is running and enable ARM POWER\n");

  potcalFlagG = 1;

  sbsControl(calProc, SBS_ON);

  return calProc;
}
