/* --- TRAJQ.C       Version 1.0
   --- This module continuously cycles through a trajectory.
   ---
   --- Copyright 2001, Mark V Automation Corp.
   ---
   --- 02/28/01 RMV     Copied from moveq for TerminatorBot.
   --- 03/05/01 RMV	Seems to work.
   ---
   --- */

#include <stdio.h>
#include <stdlib.h>	/* required for malloc() */
#include <math.h>
#include <trident\vtypes.h>
#include <trident\termbot.h>
#include <trident\trajq.h>
#include <trident\sched2.h>

#define DEF_SPEED_PARAM         100.0
#define TERMBOT_NOMINAL_SPEED   (1.0) /* nominal speed (rads/sec) at DEF_SPEED_PARAM */

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

extern float   monSpeedG;

/* ******************************************************************** */
/*      trajq Local Structure                                           */
/* ******************************************************************** */
typedef struct{ float           via[16][6];
                int             viaCnt;         /* # of vias in list */
                int             viaN;           /* current goal via */
                float           qinit[6];
                float           qdelta[6];
                int             stepN;
                float           stepSize;
                float           freq;
                } trajq_localT;

/* ******************************************************************** */
/*      trajq_on                Start up the module.                    */
/* ******************************************************************** */

int trajq_on(void *vptr)
{
  trajq_localT  *local = (trajq_localT *)vptr;
  float         max_qdelta;
  int           i, n;
  float         duration;

  /* do not execute if arm is not calibrated */

  local->qinit[0] = Q_MezG[0];
  local->qinit[1] = Q_MezG[1];
  local->qinit[2] = Q_MezG[2];
  local->qinit[3] = Q_MezG[3];
  local->qinit[4] = Q_MezG[4];
  local->qinit[5] = Q_MezG[5];

  n = 0;
  /* search the vias to find the first one far enough away from current pose */
  do{

    /* Set trajectory parameters */
    max_qdelta = 0.0;
    for (i = 0; i < 6; ++i) {
      local->qdelta[i] = local->via[n][i] - local->qinit[i];
      if (local->qdelta[i] > max_qdelta)
        max_qdelta = local->qdelta[i];
      else if (local->qdelta[i] < -max_qdelta)
        max_qdelta = -local->qdelta[i];
    }
    duration = max_qdelta * DEF_SPEED_PARAM / (TERMBOT_NOMINAL_SPEED * monSpeedG);
    local->stepN = 0;
    local->stepSize = local->freq * duration;
  } while((local->stepSize < 1.0) && (++n < local->viaCnt));

  /* Could not find a via point far enough away from current pose? */
  if (local->stepSize < 1.0)
    return SBS_OFF;
printf("tq:n = %d\n",n);
  local->viaN = n;
  local->stepSize = 1.0 / local->stepSize;

  return I_OK;
}


/* ******************************************************************** */
/*      trajq_cycle             Process module information.             */
/* ******************************************************************** */

int     trajq_cycle(void *vptr)
{
  trajq_localT  *local = (trajq_localT *)vptr;
  int           i, j, flag, n;
  double        nt, temp;
  float		max_qdelta, duration;

  flag = 1;
  /* Calculate the next set of joint positions.                         */
  if ((nt = ++local->stepN * local->stepSize) >= 1.0) {
    /* final ref joint positions have been reached */
    temp = 1.0;
    flag = 0;
  } else
    temp = nt*nt*nt*(10.0 - 15.0*nt + 6.0*nt*nt);

  for (i = 0; i < 6; i++)
    Q_RefG[i] = local->qinit[i] + (temp * local->qdelta[i]);

  if (!flag){
    /* Move to the next via point */
    local->qinit[0] = Q_RefG[0];
    local->qinit[1] = Q_RefG[1];
    local->qinit[2] = Q_RefG[2];
    local->qinit[3] = Q_RefG[3];
    local->qinit[4] = Q_RefG[4];
    local->qinit[5] = Q_RefG[5];

    n = local->viaN;
    j = 0;
    /* search the vias to find the next one far enough away from current pose */
    do{
      if (++n >= local->viaCnt)
        n = 0;
      /* Set trajectory parameters */
      max_qdelta = 0.0;
      for (i = 0; i < 6; ++i) {
        local->qdelta[i] = local->via[n][i] - local->qinit[i];
        if (local->qdelta[i] > max_qdelta)
          max_qdelta = local->qdelta[i];
        else if (local->qdelta[i] < -max_qdelta)
          max_qdelta = -local->qdelta[i];
      }
      duration = max_qdelta * DEF_SPEED_PARAM / (TERMBOT_NOMINAL_SPEED * monSpeedG);
      local->stepN = 0;
      local->stepSize = local->freq * duration;
    } while((local->stepSize < 1.0) && (++j <= local->viaCnt));

    /* Could not find a via point far enough away from current pose? */
    if (local->stepSize < 1.0)
      return SBS_OFF;

    local->viaN = n;
    local->stepSize = 1.0 / local->stepSize;
  } /* endif */
  return I_OK;
}

/* This function permits read access to the via points */
int trajq_get(processT *p_ptr, int type, int arg, void *vptr)
{
  trajq_localT  *local = (trajq_localT *)(p_ptr->local);
  float *fptr;
  int   *iptr;

  switch(type){
    case VIA_ALL:
        /* not implemented */
      fptr = (float *)vptr;
      fptr[0] = local->via[arg][0];
      fptr[1] = local->via[arg][1];
      fptr[2] = local->via[arg][2];
      fptr[3] = local->via[arg][3];
      fptr[4] = local->via[arg][4];
      fptr[5] = local->via[arg][5];
      return I_OK;
    case VIA_N:		/* return the nth via */
      fptr = (float *)vptr;
      fptr[0] = local->via[arg][0];
      fptr[1] = local->via[arg][1];
      fptr[2] = local->via[arg][2];
      fptr[3] = local->via[arg][3];
      fptr[4] = local->via[arg][4];
      fptr[5] = local->via[arg][5];
      return I_OK;
    case VIA_CNT:		/* return the viaCnt */
      iptr = (int *)vptr;
      *iptr = local->viaCnt;
      return I_OK;
  } /* endswitch */

  return I_ERROR;
}

/* This function permits write access to the via points */
int trajq_set(processT *p_ptr, int type, int arg, void *vptr)
{
  trajq_localT  *local = (trajq_localT *)(p_ptr->local);
  float *fptr;
  int   tmp,i;

  switch(type){
    case VIA_ADD:       /* Add via or list of vias to end of list */
      /* arg specifies number of vias to add */
      if (arg > 0){
	fptr = (float *)vptr;
	for (i=0; i<arg; i++){
	  tmp = local->viaCnt + i;
	  local->via[tmp][0] = fptr[6*i];
	  local->via[tmp][1] = fptr[6*i+1];
	  local->via[tmp][2] = fptr[6*i+2];
	  local->via[tmp][3] = fptr[6*i+3];
	  local->via[tmp][4] = fptr[6*i+4];
	  local->via[tmp][5] = fptr[6*i+5];
	} /* endfor */
	local->viaCnt += arg;
      } else {		/* if arg == 0, assume 1 via point */
	fptr = (float *)vptr;
	tmp = local->viaCnt;
	local->via[tmp][0] = fptr[0];
	local->via[tmp][1] = fptr[1];
	local->via[tmp][2] = fptr[2];
	local->via[tmp][3] = fptr[3];
	local->via[tmp][4] = fptr[4];
	local->via[tmp][5] = fptr[5];
	local->viaCnt++;
      } /* endif */
      return I_OK;
    case VIA_N:         /* Set nth via point */
      if (local->viaCnt <= arg)
	return I_ERROR;
      fptr = (float *)vptr;
      local->via[arg][0] = fptr[0];
      local->via[arg][1] = fptr[1];
      local->via[arg][2] = fptr[2];
      local->via[arg][3] = fptr[3];
      local->via[arg][4] = fptr[4];
      local->via[arg][5] = fptr[5];
      return I_OK;
    case VIA_DELETE:    /* Delete nth via point */
      if (local->viaCnt <= arg)
	return I_ERROR;
	/* not implemented - must re-pack the array */
      return I_OK;
  } /* endswitch */

  return I_ERROR;
}

int     trajq_init(processT *p_ptr, void *vptr)
{
  trajq_localT        *local;
  float		      *fptr;
  int i, tmp;

  /* intialize the function pointers - required */
  p_ptr->on_fptr = trajq_on;
  p_ptr->cycle_fptr = trajq_cycle;
  p_ptr->off_fptr = NULL;
  p_ptr->kill_fptr = NULL;
  p_ptr->set_fptr = trajq_set;
  p_ptr->get_fptr = trajq_get;

  /* Give the module a name - required */
  strcpy(p_ptr->modName, "TRAJQ");

  /* Allocate the local structure for the module - optional
     This struct is freed automatically */
  if ((p_ptr->local = (pointer)malloc(sizeof(trajq_localT))) == NULL){
    printf("ERROR: malloc failed\n");
    return I_ERROR;
  } /* endif */

  local = (trajq_localT *)p_ptr->local;

  /* Initialize the local structure (module dependent) */
  local->viaCnt = 0;
  local->viaN = 0;
  local->freq = p_ptr->freq;

  /* check to see if vptr holds a linear list of vias */
  if (vptr != NULL){
    fptr = (float *)vptr;
    tmp = (int)fptr[0];		/* first value is the number of vias */
				/* second value is size of vias (6 assumed) */
    for (i=0; i<tmp; i++){
      local->via[i][0] = fptr[6*i+2];
      local->via[i][1] = fptr[6*i+3];
      local->via[i][2] = fptr[6*i+4];
      local->via[i][3] = fptr[6*i+5];
      local->via[i][4] = fptr[6*i+6];
      local->via[i][5] = fptr[6*i+7];
    } /* endfor */
    local->viaCnt = tmp;
  } /* endif */

  return I_OK;
}