/*************************************************************************
 * @(#) $Header: $
 * quadratureint.c
 *
 * Objective:
 *   Accumulate 8 bit numbers from an ISR into a long.  A 32 bit
 *   long is used because the distance number can become quite
 *   large and will quickly overflow a 16 bit int.
 *
 *   We need to measure travel distance accurately.  This requires
 *   encoders with high resolution.  But since the robot drives
 *   at fairly high speed, this means the accumulator ticks come
 *   fast and furious.  The only way to keep up on the FRC is
 *   to use interrupts.  The distance data from the ISR (Interrupt
 *   Service Routine) must then be communicated to the regular
 *   code loop in the user_routines.c or user_routines_fast.c file.
 *   Doing this without hazards is tricky.
 *
 * Approach #1:
 *   Have the ISR increment a long.  The user function wanting
 *   the data simply reads the long.
 * Hazard:
 *   The PIC processor in the FRC is basically an 8 bit computer.
 *   When the user function reads the distance it reads 8 bits
 *   at a time, accumulating those bytes into a long word.
 *   If the distance value stored in the long is updated by the
 *   ISR between bytes the value received can be a composite of
 *   two different values.  For instance consider the case where
 *   the distance goes between 255 (0x00ff) and 256 (0x0100)
 *   between reading byte 0 and byte 1.  Either number is OK
 *   for navigation.  But what user code sees is the composite
 *   value, 0x01ff, which is 511.  That is a big difference and
 *   is likely to throw off the navitation algorithm.
 *
 * Solution:
 *   We need to make sure that all communication between the
 *   ISR and the user code is via atomic operations.  That
 *   means the operations cannot be interrupted.
 *
 * Approach #2:
 *   Turn off interrupts when reading the distance from user code.
 * Advantage:
 *   Simple.
 * Disadvantage:
 *   Can result in lost counts.  In general we should avoid
 *   turning off interrupts if at all possible.
 *
 * Approach #3:
 *   In the ISR increment or decrement an 8 bit value.  In user
 *   code this byte can be read atomically.  It's value must be
 *   accumulated into a much larger variable (long).
 * Advantage:
 *   Communication from the ISR to user code is atomic.  It is
 *   not necessary to turn off interrupts at any time.
 * Disadvantage:
 *   Complicated.  The long distance value must be updated from
 *   the byte value often enough.  Basically this means there
 *   should be fewer than 126 interrupts between update calls.
 *   If we consider the case of an
 *   encoder with 128 counts per rotation, if the robot drives
 *   18 inches per rotation, the robot would have to go
 *     681 inch/sec = 18 inch/rot / (128 count/rot) * 126 count/update / 26ms/update * 1000ms/s
 *   Our actual robot travels 8 ft/sec or 96 inch/sec.
 *   Its wheels are 6 inch diameter.
 *   Required update period:
 *     185 ms/update = (18 inch/rot) / (96 inch/sec) / (128 count/rot) * (126 count/update) (1000 ms/sec)
 * Conclusion:
 *   Approach #3 meets all requirements and provides margin for
 *   the expected encoder and robot performance.
 */

#include <stdio.h>
// #include <stdlib.h>
// #include "supportfuncs.h"
#include "quadratureint.h"
#define DEGREE 1
#define ABS(a) (a)>0 ? (a) : -(a)

// ----------------------------------------------------------------------------
typedef struct
{
  long totalticks;
  int dist_per_pulse;
#if defined(QUADRATUREINTDEBUG)
  unsigned int error;
  unsigned int zero;
  unsigned char lasterr; // Before and after state at last error
  unsigned char lastx; // Before and after state at last change
#endif
  unsigned char ticks8;
  unsigned char ticks8old;
  unsigned char state;
} quaddatatype;

// ----------------------------------------------------------------------------
// dist_per_pulse is in thousandths of an inch.
// newstate is the initial quadrant of the encoder.
void* quadratureint_new(int dist_per_pulse, unsigned char newstate)
{
  quaddatatype* quad_data = 0;

  // MPLab doesn't support malloc.  Use an array workaround.
#ifdef MALLOC
  quad_data = alloc( sizeof(quaddatatype) );
  if( ! quad_data )
    {
      fprintf(stderr,"quadrature_new: malloc fail.\n");
      return(0);
    }
#else
  // QUAD_QTY declares the number of quadrature encoders the software
  // will be monitoring.  Note that each quadrature encoder has two
  // sensors (optical or hall effect).  So having this set to 2 gives
  // enough sensors to monitor two wheels.  Most robots will use this
  // number.
  static quaddatatype quad_array[QUAD_QTY];
  static int quad_point=QUAD_QTY;

  if( !quad_point )
  {
//    fprintf(stderr,"quadrature_new: array allocation fail. Increase value of QUAD_QTY.\n");
    return(0);
  }
  quad_data = &quad_array[--quad_point];
#endif

  quad_data->dist_per_pulse = dist_per_pulse;
  quadratureint_init(quad_data, newstate);
  return( (void*) quad_data );
} // quadratureint_new()

// ----------------------------------------------------------------------------
void quadratureint_init(void *quad, unsigned char newstate)
{
  quaddatatype *quaddata;

  quaddata = (quaddatatype*) quad;
  quaddata->totalticks = 0L;
  quaddata->ticks8 = 0;
  quaddata->ticks8old = 0;
  quaddata->state = newstate;
#if defined(QUADRATUREINTDEBUG)
  quaddata->error = 0;
  quaddata->zero = 0;
  quaddata->lasterr = 0;
  quaddata->lastx = 0;
#endif
} //  quadratureint_init()

// ----------------------------------------------------------------------------
void quadratureint_ISR(void *quad, unsigned char newstate)
{
#if 1
  // Interrupt Service Routine for the left quadrature encoder.
  unsigned char state;
  quaddatatype *quaddata;

  quaddata = (quaddatatype*) quad;
  newstate = newstate & 0x03;
  state = quaddata->state<<2 | newstate;
  switch(state)
    {
    case 0x02: // 0b0010
    case 0x06: // 0b0110
    case 0x09: // 0b1001
    case 0x0d: // 0b1101
      quaddata->ticks8++;
#if defined(QUADRATUREINTDEBUG)
      quaddata->lastx = state;
#endif
      break;
    case 0x03: // 0b0011
    case 0x07: // 0b0111
    case 0x08: // 0b1000
    case 0x0c: // 0b1100
      quaddata->ticks8--;
#if defined(QUADRATUREINTDEBUG)
      quaddata->lastx = state;
#endif
      break;
#if defined(QUADRATUREINTDEBUG)
    case 0x0a:
    case 0x05:
    case 0x0f:
    case 0x00:
      quaddata->zero++;
      break;
    default:
      quaddata->error++;
      quaddata->lasterr = state;
      break;
#endif
    }
  quaddata->state = newstate;
#endif
  // printf("qiI: ns=%d st=%d ti=%d.\n", (int)newstate, (int)state, (int)quaddata->ticks8);
} // quadratureint_ISR()

// ----------------------------------------------------------------------------
signed char quadratureint_update(void *quad)
{
  // Update the totalticks value from the ticks8 value.
  unsigned char ticks8copy;
  signed char ticks8diff;
  quaddatatype *quaddata;

  quaddata = (quaddatatype*) quad;
  ticks8copy = quaddata->ticks8;
  ticks8diff = (signed char) (ticks8copy-quaddata->ticks8old);
  quaddata->totalticks += (long) ticks8diff;
#if 0
  printf("\tticks8diff=ticks8copy(%d,%x)-ticks8old(%d,%x);"
	 "total(%ld,%lx) += ticks8diff(%d,%x)\n",
	 (unsigned int)ticks8copy, (unsigned int)ticks8copy,
	 (unsigned int)quaddata->ticks8old,
	 (unsigned int)quaddata->ticks8old,
	 quaddata->totalticks, quaddata->totalticks,
	 (int)ticks8diff, (int)ticks8diff);
#endif

  quaddata->ticks8old = ticks8copy;
  return(ticks8diff);
} // quadratureint_update()

// ----------------------------------------------------------------------------
// Since dist_per_pulse is in 1000ths of an inch, this function
// returns inches.
INLINE long quadratureint_get(void *quad)
{
  quaddatatype *quaddata;
  long distance;

  quaddata = (quaddatatype*) quad;
  distance = quaddata->totalticks * (long)quaddata->dist_per_pulse;
  distance = distance / 1000;
  return( distance );
} // quadratureint_get()


#if defined(QUADRATUREINTDEBUG)
// ----------------------------------------------------------------------------
INLINE int quadratureint_geterr(void *quad)
{
  quaddatatype *quaddata;

  quaddata = (quaddatatype*) quad;
  return(quaddata->error);
} // quadratureint_geterr()

// ----------------------------------------------------------------------------
INLINE unsigned char quadratureint_getlasterr(void *quad)
{
  quaddatatype *quaddata;

  quaddata = (quaddatatype*) quad;
  return(quaddata->lasterr);
} // quadratureint_getlasterr()

// ----------------------------------------------------------------------------
INLINE unsigned char quadratureint_getlastx(void *quad)
{
  quaddatatype *quaddata;

  quaddata = (quaddatatype*) quad;
  return(quaddata->lastx);
} // quadratureint_getlastx()

// ----------------------------------------------------------------------------
INLINE int quadratureint_getzero(void *quad)
{
  quaddatatype *quaddata;

  quaddata = (quaddatatype*) quad;
  return(quaddata->zero);
} // quadratureint_getzero()
#endif

// ----------------------------------------------------------------------------
typedef struct
{
  quaddatatype *left, *right;
  long turndiam;
  //  int oldheading;
} quadheadingtype;

// ----------------------------------------------------------------------------
void *quadratureint_newheading(long turndiam, void *left, void *right)
{
  static quadheadingtype quadheading;

  quadheading.turndiam = turndiam;
  quadheading.left = left;
  quadheading.right = right;
  //  quadheading.oldheading = 0;

  return( (void*) &quadheading );
} // quadratureint_newheading()

/* ----------------------------------------------------------------
 * Internal use only.
 * Returns the heading in units of DEGREE from a starting heading of 0.
 * Numbers do *not* wrap at 360*DEGREE or negative.
 */
int quadratureint_getheading(void *quadheadingst)
{
  quadheadingtype *quadheading;
  int heading;
  long encoder_diff, turndiam;
  long lefttick, righttick, left, right;

  quadheading = (quadheadingtype*) quadheadingst;
  lefttick = quadheading->left->totalticks;
  left = lefttick*10000;
  left = left/quadheading->left->dist_per_pulse; // tenths of an inch
  righttick = quadheading->right->totalticks;
  right = righttick*10000;
  right = right/quadheading->right->dist_per_pulse; // tenths of an inch
  encoder_diff = left-right; // tenths of an inch
  turndiam = quadheading->turndiam/100; // tenths of an inch
  if( ABS(encoder_diff) > 100000 )
  {
    heading = encoder_diff*100;
    heading = heading/turndiam;
    heading = heading*180*DEGREE*2; // 180 degrees * 10 tenths.
    heading = heading/314; // divide pi (matching the *100 above)
  }
  else
  {
    heading = encoder_diff*180*DEGREE*2; // 180 degrees * 10 tenths.
    heading = heading/314; // divide pi (matching the *100 below)
    heading = heading*100;
    heading = heading/turndiam;
  }
  heading = (heading+1)/2; // round off (matching the 2 above).

  return( (int) heading );
} // quadratureint_getheading()

// ----------------------------------------------------------------------------