#ifndef VEHICLE_H
#define VEHICLE_H


#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string>


#define PORT  		49
#define STBD            52
#define VERTICAL        53
#define LATERAL         50

/************************** DATA ****************************************/


extern void* depth_routine(void* args);
extern float auto_heading_setpoint; 	
extern float auto_depth_setpoint; 	
extern float auto_cruise_setpoint;
extern float auto_cruise_lat_setpoint;
extern float latitude;

struct Auto_pilot {
     /* analog input from top */
     int foreaft_ch;
     int turns_ch;
     int lateral;		// hard coded in, need to change later
     int vertical;		// hard coded in, need to change later

     /* digital input from top */
     int autoHead_ch;
     int autoDepth_ch;
     int autoAltitude_ch;
     int autoCruise_ch;
     int depthJogUp_ch;
     int depthJogDown_ch;
     int bellypack_ch;

      /**** the following are coming from the bottom ***/
      int headingSource;	// 0 for gyro, 1 for compass
      int depthTransducer;	// digital channel???
      int AltitudeSource;	// 1 for alt #1, 2 for #2
      int dvlInput;
      
      /**** the following are the pid's ****************/
      float p_autoHead;
      float i_autoHead;
      float d_autoHead;
      float p_autoDepth;
      float i_autoDepth;
      float d_autoDepth;
      float p_autoAlt;
      float i_autoAlt;
      float d_autoAlt;
      float p_autoCruise;
      float i_autoCruise;
      float d_autoCruise;
      
      /**** the following are the vehicle output channels ****/
      int portThruster_ch;
      int STBDThruster_ch;
      int vertThruster_ch;
      int latThruster_ch;
      string auto_pilot_string;
      char autoPilotStr[256]; //this is the string to be sent to the vehicle
      int auto_pilot_enable;

};


struct Arm {
    /* the following are bottom output channels */
    int sin_shoulder_channel;
    int cos_shoulder_channel;
    float offset_shoulder;
    /* the following are the top input channels */
    int command_shoulder_channel_top;
    float master_shoulder;
    float feedback_shoulder; 
    int shoulder_servo_channel_bot;
    float p_shoulder;
    float i_shoulder;
    float d_shoulder;
    
    /* the following are bottom output channels */
    int sin_swing_channel;
    int cos_swing_channel;
    float offset_swing;
    /* the following are the top input channels */
    int command_swing_channel_top;
    float master_swing;
    float feedback_swing; 
    int swing_servo_channel_bot;
    float p_swing;
    float i_swing;
    float d_swing;


    /* the following are bottom output channels */
    int sin_elbow_channel;
    int cos_elbow_channel;
    float offset_elbow;
    /* the following are the top input channels */
    int command_elbow_channel_top;
    float master_elbow;
    float feedback_elbow; 
    int elbow_servo_channel_bot;
    float p_elbow;
    float i_elbow;
    float d_elbow;


    /* the following are bottom output channels */
    int sin_pitch_channel;
    int cos_pitch_channel;
    float offset_pitch;
    /* the following are the top input channels */
    int command_pitch_channel_top;
    float master_pitch;
    float feedback_pitch; 
    int pitch_servo_channel_bot;
    float p_pitch;
    float i_pitch;
    float d_pitch;

  
    /* the following are bottom output channels */
    int sin_yaw_channel;
    int cos_yaw_channel;
    float offset_yaw;
    /* the following are the top input channels */
    int command_yaw_channel_top;
    float master_yaw;
    float feedback_yaw; 
    int yaw_servo_channel_bot;
    float p_yaw;
    float i_yaw;
    float d_yaw;


    string arm_string;
    char armStr[512];
    int arm_enable;


}; 


struct Camera {

    /* the following are bottom inputs channels */
    int sin_shoulder_channel;
    int cos_shoulder_channel;
    float offset_shoulder;
    /* the following are the top input channels */
    int command_shoulder_channel_top;
    float master_shoulder;
    float feedback_shoulder;
    int shoulder_servo_channel_bot;
    float p_shoulder;
    float i_shoulder;
    float d_shoulder;

    /* the following are bottom output channels */
    int sin_pan_channel;
    int cos_pan_channel;
    float offset_pan;
    /* the following are the top input channels */
    int command_pan_channel_top;
    float master_pan;
    float feedback_pan;
    int pan_servo_channel_bot;
    float p_pan;
    float i_pan;
    float d_pan;

    /* the following are bottom output channels */
    int sin_tilt_channel;
    int cos_tilt_channel;
    float offset_tilt;
    /* the following are the top input channels */
    int command_tilt_channel_top;
    float master_tilt;
    float feedback_tilt;
    int tilt_servo_channel_bot;
    float p_tilt;
    float i_tilt;
    float d_tilt;

    string cam_string;
    char camStr[512];
    int camera_enable;
};



struct Teleos {
	
	float kf1_foreaft;
	float kf1_turns;
	float kf1_lateral;
	float kf1_vertical;
	
	float kf2_foreaft;
	float kf2_turns;
	float kf2_lateral;
	float kf2_vertical;
	
	float db_foreaft;
	float db_turns;
	float db_lateral;
	float db_vertical;
	
	float st_foreaft;
	float st_turns;
	float st_lateral;
	float st_vertical;
	
	int curve_mode;
	
	/* these feedbacks are a temporary hack until
	 * the update rate is faster from the sub
	 */

	float port_thruster;
        float stbd_thruster;
	float lateral_thruster;
	float vertical_thruster;	
	
	
	char teleos_string[1024];
	
	

};


/****************************************************************************/

class Vehicle {

   public :

   	Vehicle();
   	~Vehicle();
	char* append_auto_string();   	
	char* append_arm_string();
	char* append_cam_string();
	int update_vehicle_thruster(int index, int raw);
	
	int   gyro_validity;	
	// from parascientific
	float curr_heading;		// need this heading for
	float curr_depth;
	float curr_vel;
	float curr_lat_vel;
	float curr_raw_psi;
	float curr_pitch;
	float curr_roll;	
	float cam1_pan;
	float cam1_tilt;
	float cam2_pan;
	float cam2_tilt;
	float port_thruster;
	float stbd_thruster;
	float vert_thruster;
	float lat_thruster;      	
	float altitude;
	float autoAltitude;
	float vertSpeed;
	float heading;
	float auto_pilot;
	int bellypack_ch;
	float gesdac_output[16];
	static char auto_string[255];
	static char arm_string[255];
	static char cam_string[255];
	int mpl_digitals[65];
	int gesout_digitals[129];	
	int rpm_interval;
	int rpm_speed;
        float ship_heading;
	float dvl_altitude;
		
	struct Auto_pilot 	autopilot;
	struct Arm 		arm;
	struct Camera		camera;
	struct Teleos		teleos;
	
	int set_curr_pitch(float pitch);
   	int set_curr_roll(float roll);
   	
   	void update_depth_hist(int which_array);
	bool get_depth_hist(int which_array, char hist[]);
   	

};

extern Vehicle vehicle;			// global object

#endif
