/*****************************************************************************
Copyright 1998 MBARI                                                      
******************************************************************************
Summary  : Integrated viewing routines
Filename : SetPoint.h
Author   : Michael B. Matthews                                                
Project  : New ROV                                                        
Version  : 3.8                                                         
Created  : 11.2.98  
Modified : 23.3.98  
Notes    :                                                   
****************************************************************************/

#ifndef setpoint_h
#define setpoint_h


#define MAX_NUMBER_SETPOINTS 	8

#define DEFAULT_SETPOINT_X 	100
#define	DEFAULT_SETPOINT_Y 	0
#define DEFAULT_SETPOINT_Z 	0

#define MAX_SETPOINT_PAN 	DEGS_TO_RADS(150)
#define MIN_SETPOINT_PAN 	DEGS_TO_RADS(-150)
#define MAX_SETPOINT_TILT 	DEGS_TO_RADS(45)
#define MIN_SETPOINT_TILT 	DEGS_TO_RADS(-45)

#define MAX_SETPOINT_RATE_PAN 	DEGS_TO_RADS(40)
#define MAX_SETPOINT_RATE_TILT 	DEGS_TO_RADS(30)
#define MAX_SETPOINT_RATE_FOCUS 100

#define SETPOINT_LOW_SENSITIVITY	1.0
#define SETPOINT_MEDIUM_SENSITIVITY	0.33
#define SETPOINT_HIGH_SENSITIVITY	0.1
#define SETPOINT_DEFAULT_SENSITIVITY	1.0

// joystick model parameters 20Hz
#define A11	1.0
#define A12	0.016
#define A21	0.0
#define A22	0.025
#define B1	0.02
#define B2	0.3

enum { RATE_MODE, POSITION_MODE };
enum { RECTANGULAR, POLAR };


// friend functions
Flt32 Focus_to_Focal_Distance(Flt32);
Flt32 Focal_Distance_to_Focus(Flt32);

class PTControlSource;


/******************************************************************************
Class   : S E T P O I N T                                              
Purpose : This object represents a point in 3D Euclidean space. 
Notes   : The setpoint is moved in polar coordinates using the three control 
        : sources for the pan, tilt, and focus axes. The focus control axis 
        : corresponds to the distance coodinate.                               
******************************************************************************/

class SetPoint 
{
 public:
  Int i;
  Int index;
  MBool	update;				
  Flt64 time;								
  Word	deviceMap;			// device map for selected devices
  Device* refDevice;			// reference device for setpoint movement
  Flt32 f, fd, pan, tilt;		// position in spherical coordinates 
  Flt32 dfd, dPan, dTilt;	        // velocity in spherical coordinates
  Flt32	x, y, z;			// position in Cartesian coordinates
  Flt32	dx, dy, dz;			// velocity in Cartesian coordinates
  Flt32 xOrigin, yOrigin, zOrigin;	// setpoint origin
  Flt32 maxSetPointPan;			
  Flt32 minSetPointPan;			
  Flt32 maxSetPointTilt;		
  Flt32 minSetPointTilt;		
  Flt32 maxSetPointFocus;		
  Flt32 minSetPointFocus;		

  PTControlSource* controlSource;	// control src for pan-tilt axes

  SetPoint(Flt32 x = DEFAULT_SETPOINT_X, 
	   Flt32 y = DEFAULT_SETPOINT_Y, 
	   Flt32 z = DEFAULT_SETPOINT_Z,
	   Int coord = RECTANGULAR) { Init(x,y,z,coord); }
  SetPoint(SetPoint* setPoint);		
  ~SetPoint();	
  Void Init(Flt32 x = DEFAULT_SETPOINT_X, 
	    Flt32 y = DEFAULT_SETPOINT_Y, 
	    Flt32 z = DEFAULT_SETPOINT_Z,
	    Int coord = RECTANGULAR);

  SetPoint& operator=(const SetPoint& setPoint);

  Void Register();
  Void Unregister();
  Void Set(Flt32 xi, Flt32 yi, Flt32 zi, Int coord = RECTANGULAR);
  Void SetOrigin(Flt32 xi = 0, Flt32 yi = 0, Flt32 zi = 0) 
                { xOrigin = xi; yOrigin = yi; zOrigin = zi; }

  STATUS Update(Flt32 uPan, Flt32 uTilt, Int mode, IviewSensitivity s);
  STATUS Remap(PTControlSource*);
  STATUS Add(Word device);
  STATUS Remove(Word device);

  inline Word 	DeviceMap() { return deviceMap; }
  inline Void 	DeviceMap(Word device) { deviceMap = device; }
  inline Flt32 	Distance(Flt32 focus);
  inline Flt32 	positionX() { return x; }
  inline Flt32 	positionY() { return y; }
  inline Flt32 	positionZ() { return z; }
  inline Flt32 	positionFD() { return fd; }
  inline Flt32 	positionPan() { return pan; }
  inline Flt32 	positionTilt() { return tilt; }

  Flt32	 Focus();
  Flt32  InputFunction(Flt32);
  Void   Reference(Word reference);
  Void   Reference(Device* reference);
  Void   Rect2Polar();
  Void 	 Polar2Rect();
  Void   Limit();
  Void   Print();

  friend Flt32 Focus_to_Focal_Distance(Flt32);
};


#endif