#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    5000
#define ACT_LIMIT (((long int) PIDSCALE)*95)

		
typedef struct
{
	byte channel;	 		//servo channel
	enum servoOption optionMask;
        enum servoStatusBits statusMask;//current servo status
	enum servoReason reason;  //the information sent back at the completion of a move
	struct servoStatus status;      //status latched after err
	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 thresholdOffset; //at threshold goal := position + this
 
	int32 lastIntermediate; 	//previous value of intermediate
	int32 lastDerivative;
	int32 goalPosition; 	//desired final position
	int32 relative;			//size of relative move in encoder counts
	int32 intermediate;	 	//desired position for next servo cycle
	int32 maxPositionErr;
	int16 pressure1;
	int16 pressure2;

	uint16 maxSpeed; 		//plateau speed
	uint16 defaultMaxSpeed;
	uint16 minSpeed;
	int16 cmdVel;	 		//current velocity
	uint16 accel;	 		//constant accel rate 
	int8 stopFlag;			//deceleration flag
	
	int16PID gain;	
	int32 sum;		 		//sum of error for integral term
	int32 previousSum;
	int32 lastError;		//previous error for derivative term
	
	int16 motorCurrent;		//motor current in counts
	int16  maxPressureDelta;	//max difference between pressures
	int16 maxInPressure;	//max acceptable absolute pressure
	int16 maxOutPressure;	//max acceptable absolute pressure
	int16 minInPressure;	//max acceptable absolute pressure
	int16 minOutPressure;	//min acceptable absolute pressure
	int16 maxCurrent;
	byte limitedFlag;
    byte pressureSenseSwitch;
    int32 latchPosition;
    uint16 servoTics;
    uint16 repeatRate;
	
}MotorChannel;

typedef void trapFn (MotorChannel *m);

extern trapFn *MoveType0, *MoveType1;

void getNewPos(MotorChannel* mc);
void getLatchPos(MotorChannel* mc);
int32 getPosition(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 startAbsMove(MotorChannel* mcChan, int32 newPosition,
                unsigned tag, unsigned source);
void startRelMove(MotorChannel* mcChan, int32 newPosition,
                unsigned tag, unsigned source);
void MotorOverCurrent(MotorChannel* mcChan);
void MotorPositionError(MotorChannel* mcChan);
void servoOff(MotorChannel* mcChan);

#endif