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

#ifndef device_h
#define device_h


// friend functions
Void logControllers();

class Focus;
class Device;
class Light;
class Camera;
class CameraControlSource;
class PTControlSource;
class SetPoint;
class Axis;


/*****************************************************************************
Class   : D E V I C E  C O N F I G                                             
Purpose : Struct that contains all device configuration data
Notes   : This struct is configured from configuration file iview.cfg in 
        : routine iviewConfigFile() in iview.C.
******************************************************************************/

struct DeviceConfig
{
  // device origin
  FltN xOrigin;
  FltN yOrigin;
  FltN zOrigin;

  // device initial setpoint 
  FltN xSetPoint;
  FltN ySetPoint;
  FltN zSetPoint;

  // pan/tilt limits 
  FltN posLimitPan;
  FltN negLimitPan;
  FltN posLimitTilt;
  FltN negLimitTilt;

  // velocity limits
  FltN maxVelocityPan;
  FltN minVelocityPan;
  FltN maxVelocityTilt;
  FltN minVelocityTilt;

  // velocity to pt command conversion
  FltN velocityConversion;

  // PID loop gains
  FltN pGainPan;
  FltN dGainPan;
  FltN iGainPan;
  FltN fGainPan;
  FltN pGainTilt;
  FltN dGainTilt;
  FltN iGainTilt;
  FltN fGainTilt;

  // device offsets 
  FltN offsetPan;
  FltN offsetTilt;

  // offset of device from center of rotation
  FltN bracketOffsetPan;	
  FltN bracketOffsetTilt;

  // initial focal length
  FltN initialFocalLength;

  // device panoramic mode offsets
  FltN panoramicOffsetPan;
  FltN panoramicOffsetTilt;

  // device buffer length
  Int16 bufferLength;
};



/*****************************************************************************
Class   : D E V I C E                                              
Purpose : Base class for all devices 
Notes   : Derived classes are cameras and lights 
******************************************************************************/

enum { PAN, TILT, BOTH, FOCUS };

class Device 
{
public:
  MBool comLinkError;
  IviewDevice device;			// device id
  Word status;				// device status
  String dmName;			// device name
  FILE* fp;				// MBM temp for logging
  FltN x,y,z;				// position in device coordinates
  FltN dx,dy,dz;			// derivative of position
  FltN d,f,fd,df;			// 
  FltN psi,theta;			//		
  FltN dpsi,dtheta;			//		
  FltN xOrigin,yOrigin,zOrigin;	// device origin
  FltN l;				// offset from center of rotation

  SetPoint* setPoint;			// pointer to connected setpoint
  Axis* pan;				// pan axis
  Axis* tilt;				// tilt axis

  Device(IviewDevice device,		// device id
	 String name,			// device name
	 DeviceConfig* cfg)		// device configuration parameters
    : dmName(name) { Init(device, cfg); }

  Device(IviewDevice device,		// device id
	 char* name,			// device name
	 DeviceConfig* cfg)		// device configuration parameters
    : dmName(name) { Init(device, cfg); }

  ~Device() { delete pan; delete tilt; }

  STATUS Init(IviewDevice dev,		// device id
	      DeviceConfig*);		// device configuration parameters
  String  nameConversion(String&);

  virtual STATUS Print();
  virtual STATUS Update(); 		
  virtual STATUS Move(FltN pan = 0, FltN tilt = 0);	
  virtual STATUS Position(FltN pan = 0, FltN tilt = 0);	
  virtual STATUS enableControl(Int = BOTH);
  virtual STATUS disableControl(Int = BOTH);
  virtual STATUS Remap(SetPoint* sp);
  virtual STATUS logParameters(char* name, MBool firstCall = FALSE);
  virtual STATUS logParameters(String& name, MBool firstCall = FALSE)
                 { logParameters(name.str, firstCall); }
  virtual STATUS FGain(Flt64 f) { pan->FGain(f); tilt->FGain(f); }
  virtual STATUS Stop() { return Move(0,0); }	
  virtual Void Display();
  virtual Void Log() { pan->Log(); tilt->Log(); }
  virtual Void DirectWrite(MBool d) { pan->DirectWrite(d); tilt->DirectWrite(d); }
  virtual Void setPanoramic() { pan->setPanoramic(); tilt->setPanoramic(); }
  virtual Void resetPanoramic() { pan->resetPanoramic(); tilt->resetPanoramic(); }
};   



/******************************************************************************
Class   : L I G H T                                              
Purpose : Light object class 
Notes   : Light is a derived class form Device class
******************************************************************************/

#define POWER_DM "POWER"

class Light : public Device 
{
public:
  Int16 power;
  DmGroup* group;
  DmInt16Object* powerDmi;

  Light(IviewDevice device,		
	String& dmName,
	DeviceConfig* cfg)
    : Device(device, dmName, cfg) { Light::Init(cfg); }

  Light(IviewDevice device,	
	char* dmName,
	DeviceConfig* cfg)
    : Device(device, dmName, cfg) { Light::Init(cfg); }

  ~Light() { delete powerDmi; delete group; }

  STATUS Init(DeviceConfig* cfg);
  STATUS Update(MBool comLinkError = FALSE);
};



/******************************************************************************
Class   : F O C U S                                              
Purpose : Camera focus object class 
Notes   : Focus object belongs to camera object
******************************************************************************/

#define DEFAULT_FOCAL_DISTANCE 	10.0
#define DEFAULT_FOCUS_SETPOINT	1
#define MAX_SETPOINT_FOCUS 	300.0	// maximum focal distance in inches
#define MIN_SETPOINT_FOCUS 	16.0	// minimum focal distance in inches

class Focus 
{
private:
  Flt32 fd;			// focal distance
  Flt32 focus;			// focus value in [0,1]
  Int16 setPoint;		// setpoint on camera lens servo
  Int16 setPointLast;		// setpoint on camera lens servo
  Int16 position;		// position feedback from lens servo pot

  DmGroup* group;
  DmInt16Object* setPointDmi;
  DmFlt32Object* focalDistanceDmi;
  DmInt16Object* positionDmi;

public:
  CameraControlSource* controlSource;
  CalibrationTable* focalLengthTable;

  Focus(String& dmName) { Init(dmName); }
  Focus(char* dmName) { String name(dmName); Init(name); }
  ~Focus() { delete group; delete setPointDmi; 
	     delete positionDmi; delete focalLengthTable; }

  Void Init(String&);
  STATUS Update(Flt32 d) { focus = Focal_Distance_to_Focus(d); Update(); }
  STATUS Update();
  STATUS Position(Flt32 d) { focus = d; }
  STATUS Rate(Flt32 d) { focus = d; }
  STATUS Log(FILE* fp, String& string); 	
  STATUS Remap(CameraControlSource* = NULL);
  Flt32  Focal_Distance_to_Focus(Flt32 fd);
  Flt32  Focus_to_Focal_Distance(Flt32);
  Flt32  focalDistance() { return fd; }
  Flt32  focusPosition() { return focus; }
};



/******************************************************************************
Class   : Z O O M                                              
Purpose : Camera zoom object class 
Notes   : Zoom object belongs to camera object
******************************************************************************/

#define ZOOM_MAX_SETPOINT  	1023
#define ZOOM_MIN_SETPOINT  	0
#define ZOOM_MAX_RATE	  	50
#define ZOOM_MIN_RATE	  	-50
#define ZOOM_RATE_CONSTANT 	0.025
#define ZOOM_DEFAULT_SETPOINT	0
#define ZOOM_RATE_FULL_SCALE	50
#define ZOOM_POSITION_MODE 	0
#define ZOOM_RATE_MODE 		1

class Zoom 
{
private:
  Word mode;		// rate or position mode
  Int16 position;	// position feedback from lens servo pot
  Int16 rate, rateLast;	// rate mode input
  Int16 setPoint, setPointLast;
  DmGroup* group;

  DmNat16Object* modeDmi;
  DmInt16Object* setPointDmi;
  DmInt16Object* positionDmi;

public:
  Zoom(String& dmName) { Init(dmName); }
  Zoom(char* dmName) { String name(dmName); Init(name); }
  ~Zoom() { delete group; delete setPointDmi; delete positionDmi; delete modeDmi; }

  Void Init(String&);
  STATUS Update();
  STATUS Position(Flt32 z);
  STATUS Rate(Flt32 z);
  Flt32 Position() { return ((Flt32) position)/(ZOOM_MAX_SETPOINT-ZOOM_MIN_SETPOINT); }
};



/******************************************************************************
Class   : I R I S                                              
Purpose : Camera iris object class 
Notes   : Iris object belongs to camera object
******************************************************************************/

#define IRIS_MAX_SETPOINT  	900.0/IRIS_FULL_SCALE
#define IRIS_MIN_SETPOINT  	0.0/IRIS_FULL_SCALE
#define IRIS_RATE_CONSTANT 	0.01
#define IRIS_DEFAULT_SETPOINT	500/IRIS_FULL_SCALE

class Iris 
{
private:
  MBool mode;		// modes: normal, auto
  Flt32 setPoint;	// setpoint on camera lens servo
  Flt32 rate;		// rate mode input
  Int16 sp, spLast;	
  DmGroup* group;

  DmBooleanObject* modeDmi;
  DmInt16Object* setPointDmi;

public:
  Iris(String& dmName) { Init(dmName); }
  Iris(char* dmName) { String name(dmName); Init(name); }
  ~Iris() { delete group; delete setPointDmi; delete modeDmi; }

  Void Init(String&);
  STATUS Update();
  STATUS Position(Flt32 z) { setPoint = z; return OK; }
  STATUS Rate(Flt32 z) { rate = z; return OK; }
};



/******************************************************************************
Class   : C A M E R A                                               
Purpose : Camer object class 
Notes   : Derived class from class Device
******************************************************************************/

class Camera : public Device 
{
public:
  Focus* focus;
  Zoom* zoom;
  Iris* iris;

  Camera(IviewDevice device, 
	 String& dmName, 
	 DeviceConfig* cfg)
    : Device(device, dmName, cfg) { Camera::Init(cfg); }

  Camera(IviewDevice device, 
	 char* dmName, 
	 DeviceConfig* cfg)
    : Device(device, dmName, cfg) { Camera::Init(cfg); }

  ~Camera() { delete focus; delete zoom; delete iris; }

  STATUS Init(DeviceConfig* cfg);
  STATUS Update(MBool comLinkError = FALSE);
  STATUS logParameters(char*, MBool firstCall = FALSE);
  STATUS logParameters(String& name, MBool firstCall = FALSE)
  { logParameters(name.str, firstCall); }
  STATUS Print();
};



/******************************************************************************
Class   : C O N T R O L L E R                                              
Purpose : This object represents a complete pan/tilt controller unit. 
Notes   : The controller unit controls two devices.
        : The standby function 
******************************************************************************/

class Controller 
{
public:
  MBool comLinkError;
  MBool standby;
  String location;
  Int16 velocityCmd[4];

  Camera* camera;
  Light* light;

  DmGroup* group;
  DmInt16Object* velocityCmdDmi;
  DmBooleanObject* comLinkErrorDmi;

  Controller(char* name,
	     IviewDevice cameraDevice, 
	     DeviceConfig* cameraConfig,
	     IviewDevice cameraDevice, 
	     DeviceConfig* lightConfig);
  ~Controller();
  STATUS Update();
  STATUS Velocity();
  STATUS Log();
  STATUS logParameters();
  Void 	 Stop() { camera->Stop(); light->Stop(); Velocity(); }
  Void   enableControl() { camera->enableControl(); light->enableControl(); }
  Void   disableControl() { camera->disableControl(); light->disableControl(); }
  Void   DirectWrite(MBool d) { camera->DirectWrite(d); light->DirectWrite(d); }
  Void   Standby() { standby = TRUE; Stop(); }
  Void   Resume() { standby = FALSE; }
  Void   Start() { standby = FALSE; }
  MBool  Status() { if (standby || comLinkError) return FALSE; else return TRUE; }
};


#endif