#include "vehicle.h"
#include "thruster_curves.h"
#include <math.h>
#include "control.h"

Vehicle vehicle;

float auto_heading_setpoint = 0.0;
float auto_depth_setpoint = 0.0;
float auto_cruise_setpoint = 0.0;
float auto_cruise_lat_setpoint;
float auto_cruise_use_bottom_lock;
float latitude = 0.0;
float array1[100];	/* 2 minutes */
float array2[50];	/* 30 minutes */
float array3[25];	/* 120 minutes */
float pitch_old[5];
float roll_old[5];

static int dcount1 = 0;
static int dcount2 = 0;
static int dcount3 = 0;

extern int depth_hist_thread_timer;
	
Vehicle::Vehicle()
{

    /* the following are the top input channels */
    arm.command_shoulder_channel_top = -1;
    arm.command_swing_channel_top = -1;
    arm.command_elbow_channel_top = -1;
    arm.command_pitch_channel_top = -1;
    arm.command_yaw_channel_top = -1;
    arm.arm_string = "\0";
    arm.arm_enable = 0;           	// initially arm enable should be off

    /* the following are the top input channels */
    camera.command_shoulder_channel_top = -1;
    camera.command_pan_channel_top = -1;
    camera.command_tilt_channel_top = -1;
    camera.cam_string = "\0";
    camera.camera_enable = 0;          	// initially camera enable should be off



    autopilot.auto_pilot_string = "\0";
    autopilot.lateral = 1;		// hard coded in
    autopilot.vertical = 3;		// hard coded in
    autopilot.auto_pilot_enable = 0;   	// initially autopilot enable should be off
    autopilot.headingSource = 0;

    bellypack_ch = 1;              	// hard coded in
    port_thruster = 0.0;
    stbd_thruster = 0.0;
    vert_thruster = 0.0;
    lat_thruster = 0.0;
    rpm_interval = 1;
    rpm_speed = 0;
    curr_vel = 0.0;
    curr_lat_vel = 0.0;
    
    for (int i = 0; i < 65; i++)
    	mpl_digitals[i] ;

    for (int i = 0; i < 129; i++)
    	gesout_digitals[i] ;
    	
    for (int i = 0; i < 16; i++)
    	gesdac_output[i] = 0;
    		
    	// Initialize teleos variable
     	teleos.kf1_foreaft = 925920;
	teleos.kf1_turns = 925920;
	teleos.kf1_lateral = 15840;
	teleos.kf1_vertical = 26400;
	
	teleos.kf2_foreaft = 1339560;
	teleos.kf2_turns = 1339560;
	teleos.kf2_lateral =  78400;
	teleos.kf2_vertical = 108900;
	
	teleos.db_foreaft = 0.5;
	teleos.db_turns = 0.5 ;
	teleos.db_lateral = 0.5;
	teleos.db_vertical = 0.5;
	
	teleos.st_foreaft = 0;
	teleos.st_turns = 0;
	teleos.st_lateral = 0.25;
	teleos.st_vertical = 0.25;
	
	teleos.curve_mode = 1;
		

}

Vehicle::~Vehicle()
{

}
  
char* Vehicle::append_cam_string()
{
	snprintf(camera.camStr, 512,
           "%d|%d|%d|%d|%d|%d|%d|%d|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%d|%5.4f|%5.4f|%5.4f\n",
            SET_CAM, camera.shoulder_servo_channel_bot,camera.pan_servo_channel_bot,camera.tilt_servo_channel_bot,
	    camera.sin_shoulder_channel, camera.cos_shoulder_channel,
	    camera.sin_pan_channel, camera.cos_pan_channel,
	    camera.sin_tilt_channel, camera.cos_tilt_channel,
	    camera.p_shoulder, camera.d_shoulder, camera.i_shoulder,
	    camera.p_pan, camera.d_pan, camera.i_pan,
	    camera.p_tilt, camera.d_tilt, camera.i_tilt,  	
	    camera.camera_enable,
	    camera.offset_shoulder,camera.offset_pan,camera.offset_tilt);
	
	    return camera.camStr;
}

// Called by send all data down on start up
//Not called by commands.c to update the vehicle
char* Vehicle::append_auto_string()
{
    	snprintf(autopilot.autoPilotStr, 256,
    	     "%d|%d|%d|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%d|%d\n",
             SET_PILOT, autopilot.headingSource, autopilot.depthTransducer, autopilot.AltitudeSource, autopilot.dvlInput,
	     autopilot.p_autoHead, autopilot.i_autoHead, autopilot.d_autoHead, autopilot.p_autoDepth, autopilot.i_autoDepth,
             autopilot.d_autoDepth, autopilot.p_autoAlt, autopilot.i_autoAlt, autopilot.d_autoAlt,
             autopilot.p_autoCruise, autopilot.i_autoCruise, autopilot.d_autoCruise,
             autopilot.portThruster_ch, autopilot.STBDThruster_ch, autopilot.vertThruster_ch, autopilot.latThruster_ch);

             return autopilot.autoPilotStr;
}


char* Vehicle::append_arm_string()
{
    	snprintf(arm.armStr, 512,
           "%d|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%d|%d|%d\n",
            SET_ARM, arm.sin_shoulder_channel, arm.cos_shoulder_channel, arm.offset_shoulder,
	    arm.p_shoulder, arm.d_shoulder, arm.i_shoulder,
	    arm.sin_swing_channel, arm.cos_swing_channel, arm.offset_swing,
	    arm.p_swing, arm.d_swing, arm.i_swing,
	    arm.sin_elbow_channel, arm.cos_elbow_channel, arm.offset_elbow,
	    arm.p_elbow, arm.d_elbow, arm.i_elbow,
	    arm.sin_pitch_channel, arm.cos_pitch_channel, arm.offset_pitch,
	    arm.p_pitch, arm.d_pitch, arm.i_pitch,
	    arm.sin_yaw_channel, arm.cos_yaw_channel, arm.offset_yaw,
	    arm.p_yaw, arm.d_yaw, arm.i_yaw,
	    arm.shoulder_servo_channel_bot,
	    arm.swing_servo_channel_bot,
	    arm.elbow_servo_channel_bot,
	    arm.pitch_servo_channel_bot,
	    arm.yaw_servo_channel_bot);

	return arm.armStr;
}


int
Vehicle::update_vehicle_thruster(int index, int raw)
{

	// The raw value here is still an integer raw
	// This contants (PORT, ...) are defined in vehicle.h
	// This array will be sent to logging string and vision string
	// inside telemetry_string.cpp
	// The gesdac are monitored by gesadc input 49 through 64
	
 	gesdac_output[index-49] = (raw * 10.0)/4095 - 5.00;
	switch (index)
	{
	 	case PORT:
	 		port_thruster = (raw * 10)/4095 - 5.00;
	 		break;
	 	case STBD:
	 	        stbd_thruster = (raw * 10)/4095 - 5.00;	 		
	 		break;
	 	case VERTICAL:
	 		vert_thruster = (raw * 10)/4095 - 5.00;	 		
	 		break;
	 	case LATERAL:
	 	        lat_thruster = (raw * 10)/4095 - 5.00;	 		
	  		break;
	  	default:
	  		break;
	} 	
        return 1;
}



int Vehicle::set_curr_pitch(float pitch)
{
	float change;	

	if (pitch > 12 || pitch < -12) return -1;

	pitch_old[4] = pitch_old[3];
	pitch_old[3] = pitch_old[2];
	pitch_old[2] = pitch_old[1];
	pitch_old[1] = pitch_old[0];
	pitch_old[0] = pitch;
	
	curr_pitch = (pitch_old[4] + pitch_old[3] + pitch_old[2] + pitch_old[1] + pitch_old[0])/ 5;
	return 1;
}

int Vehicle::set_curr_roll(float roll)
{
	float change;
	if (roll > 12 || roll < -12) return -1;

	roll_old[4] = roll_old[3];
	roll_old[3] = roll_old[2];
	roll_old[2] = roll_old[1];
	roll_old[1] = roll_old[0];
	roll_old[0] = roll;
	
	curr_roll = (roll_old[4] + roll_old[3] + roll_old[2] + roll_old[1] + roll_old[0])/ 5;
	return 1;
}

/***************************************************************
* Thread to update the depth history arrays
*
*
*
****************************************************************/
void *depth_routine(void *args)
{
	int count1 = 0;
	int count2 = 0;
	int count3 = 0;

	while(1){
		if (count1 >= 2)
			{
			// every 2 seconds
			vehicle.update_depth_hist(1);	
			count1 = 0;
			}
		if (count2 >= 30)
			{
			// every 30 seconds
   			vehicle.update_depth_hist(2);	
			count2 = 0;
			}
		if (count3 >= 120)
			{
			// every 2 minutes
   			vehicle.update_depth_hist(3);	
  			count3 = 0;
			}
		sleep(1);
		count1++;
		count2++;
		count3++;	
		if(depth_hist_thread_timer < 1000) depth_hist_thread_timer++;
		}
	return NULL;	// never return, just to keep compiler happy
}


void Vehicle::update_depth_hist(int which_array)
{
	switch(which_array) {
	   case 1:
		if (dcount1 >= 100) dcount1 = 0;
		array1[dcount1++] = vehicle.curr_depth;
		break;
	   case 2:
		if (dcount2 >= 50)  dcount2 = 0;
		array2[dcount2++] = vehicle.curr_depth;
		break;
	   case 3:
		if (dcount3 >= 25)  dcount3 = 0;
		array3[dcount3++] =  vehicle.curr_depth;
		break;
	   default:
		return;
	}
}


/*
 * This is going to be called by commands.c where "result"
 * is this hist[]
 */

bool Vehicle::get_depth_hist(int which_array, char hist[])
{
	int index;
	int max_count;
	int need_padding = 0;
	int ntimes = 0;
	float* array;
	char   temp[128];

	switch(which_array) {
	   case 1:
		index = dcount1 - 1;
		array = array1;
		max_count = 100;
		break;
	   case 2:
		index = dcount2 - 1;
		array = array2;
		max_count = 50;
		break;
	   case 3:
		index = dcount3 - 1;
		array = array3;
		max_count = 25;
		break;
	   default:
		return false;
	}

	bzero(hist, strlen(hist));
	for (ntimes, index; index >= 0 && ntimes < 25; ++ntimes, --index)
		{
		snprintf(temp, 128, "%4.2f|", array[index]);	
		strcat(hist, temp);
		}
	max_count--;
	for(ntimes; ntimes < 25; ntimes++)
		{
		snprintf(temp, 128, "%4.2f|", array[max_count]);
		max_count--;
		strcat(hist, temp);
		}	
        //cout <<"depth hist : "<<hist<<endl;
 	return true;
}
