#ifndef _TrapServo
#define _TrapServo
#include "dwarf.h"
#include "servobits.h"
#include "espmsg.h"
#define VEL_LIMIT	120	
#define INT_LIMIT	((long int)32768)
#define PIDSCALE    4096  //use a power of 2 for efficient division
#define ACT_LIMIT (((long int) PIDSCALE)*70)
#define VEL_LIMITING_HIGH 85
#define VEL_LIMITING_LOW 75
		
typedef struct
{
   byte channel;	 		//servo channel
   enum {
     limited = 0x1,
     pressureSenseSwitch = 0x2,
     pressureErrorSent = 0x4,  //pressure error has been sent, don't resend
   } flags;

   enum servoOption optionMask;
   enum servoStatusBits statusMask;//current servo status
   enum servoReason reason;  //the information sent back at the completion of a move
   uint8 source;          // add these to hold who reguested the current move
   uint8 tag;             // if this field is 0 then no move in progress
   uint8 cmdType;         //rel or absolute
   uint8 sourceStatus;    // add these to hold who reguested the current move
   uint8 tagStatus;         // if this field is 0 then no move in progress

   int32 position;  		//absolute position (not encoder reading)
   int32 lastPosition;          //last value of position
   int32 homePosition;          //position at the home flag's transition
   int32 thresholdOffset;       //at threshold goal := position + this

   int32 goalPosition; 	        //desired final position
   int32 relative;	        //size of relative move in encoder counts
   int32 intermediate;	 	//desired position for next servo cycle
   int32 maxPositionErr;
   uint16 inPressure;
   uint16 outPressure;

   uint16 maxSpeed; 		//plateau speed
   uint16 defaultMaxSpeed;
   uint16 minSpeed;
   uint16 maxSettling;          //maximum settling time in tics
   int16 cmdVel;	 	//current velocity
   uint16 accel;	 	//constant accel rate 

   int16PID gain;	
   int32 sum;		 	//sum of error for integral term
   int32 lastError;		//previous error for derivative term

   int16 motorCurrent;		//motor current in counts
   uint16 maxInPressure;	//max acceptable absolute pressure
   uint16 maxOutPressure;	//max acceptable absolute pressure
   uint16 minInPressure;	//min acceptable absolute pressure
   uint16 minOutPressure;	//min acceptable absolute pressure
   uint16 maxCurrent;

#ifdef PRESSURE_DELTA_IMPLEMENTED  //not yet implemented, return 0
   uint16 maxPressureDelta;	//max difference between pressures
   uint16 minPressureDelta;	//min difference between pressures
#endif
   
   int32 latchPosition;
   uint16 servoTics;
   uint16 repeatRate;
   int16 PIDavg; 
   int32 avgSpeed;
   int32 DERRavg;
   uint16 settleCounter;
   struct servoStatus status;      //status latched after err

} MotorChannel;

typedef void trapFn (MotorChannel *m);

extern trapFn *MoveType0, *MoveType1;

void getNewPos(MotorChannel* mc);
int32 getLatchPos(MotorChannel* mc);
enum servoStatusBits updateStatus(MotorChannel *mc);
void trapServoLoop(MotorChannel* mc);
void configMotorChannel(MotorChannel* mc,byte channel);
void initServo(void);
void ServoDoNothing(MotorChannel* mc);
void MotorOverCurrent(MotorChannel* mcChan);
void MotorPositionError(MotorChannel* mcChan, enum servoReason reason);
void MotorPressureError(MotorChannel* mcChan, long int stop_distance);
void servoOff(MotorChannel* mcChan, int pwm);

//start either relative or absolute move depending on cmd
void startServoMove(unsigned source, unsigned tag, unsigned cmd,
                    MotorChannel* mcChan, int32 offset);

void errorOut(MotorChannel* mcChan);

static int32 inline updateAvgSpeed (MotorChannel* mcChan)
{
  int32 velocity = mcChan->position-mcChan->lastPosition;
  mcChan->avgSpeed = (3*mcChan->avgSpeed + abs(velocity))/4;
  mcChan->lastPosition=mcChan->position;
  return velocity;
}

#endif