#include "depth.h"
#include <math.h>
#include "vehicle.h"
#include "telemetry.h"

// Determine which array the data should be inserted in
#define DEPTH_A1	1
#define DEPTH_A2	2
#define DEPTH_A3	3

// Time to get 25 values in minutes
#define TIME1		2 	// min
#define TIME2		30    	// min
#define TIME3		120    	// min


Tel_Depth td;
int telemetry_status = 0;
int receiving_data_counter = 2;
extern int tty_lineout(char*);		// from telemetry.h
extern int tty_send_request(char*);	// from telemetry.h
extern int head_hist_and_telem_timer_thread_timer;

/**********************************************************************/	
// get 25 data every 2 minutes
// 120 sec / 25 = 4.8

/********************************************************************
* Updates Heading History array
* Times the received data from the telemetry to display an error on timeout
*
*********************************************************************/
void *depth_head_routine(void *args)
{
	char ping[] = "PING\n";
	extern int head_hist_and_telem_timer_thread_timer;
	while(1) {
	
 		td.update_head_hist();	      		
		sleep(1);
		
		// decrement the counter to check whether the bottom still alive
		if (receiving_data_counter > 0)
			receiving_data_counter--;
		else {
			// sub is not transmitting, tell the top to
			// display red light and then kill and restart
			// the receiving thread again
			telemetry_status = 0;
			receiving_data_counter = 0;
		}
                if(head_hist_and_telem_timer_thread_timer < 100000) head_hist_and_telem_timer_thread_timer++;
	}

	return NULL;	// never return, just to keep compiler happy
}




Tel_Depth::Tel_Depth()
{

   // initialize the array to be -1 when it has no data
   for (int i = 0; i < MAXCOUNT1; i++)
   {
   	array1[i] = -1;
   }
   for (int i = 0; i < MAXCOUNT2; i++)
   {
   	array2[i] = -1;
   }
   for (int i = 0; i < MAXCOUNT3; i++)
   {
   	array3[i] = -1;
   }

   for (int i = 0; i < MAXCOUNT2; i++)
   {
   	headArray[i] = -1;
   }

   count1 = count2 = count3 = headCount = 0;

}

Tel_Depth::~Tel_Depth()
{


}

void
Tel_Depth::update_depth_hist(int which_array)
{

	switch(which_array) {
	   case DEPTH_A1:
		if (count1 >= MAXCOUNT1)
			count1 = 0;
		array1[count1++] = vehicle.curr_depth;
		break;
	   case DEPTH_A2:
		if (count2 >= MAXCOUNT2)
			count2 = 0;
		array2[count2++] = vehicle.curr_depth;
		break;
	   case DEPTH_A3:
		if (count3 >= MAXCOUNT3)
			count3 = 0;
		array3[count3++] =  vehicle.curr_depth;
		break;
	   default:
		return;
	}
	
}


void
Tel_Depth::update_head_hist()
{
	extern int deviation_fog;
	if (headCount >= MAXCOUNT2)
		headCount = 0;
	headArray[headCount++] = vehicle.curr_heading + deviation_fog;

}


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

bool
Tel_Depth::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 DEPTH_A1:
		index = count1 - 1;
		array = array1;
		max_count = MAXCOUNT1;
		break;
	   case DEPTH_A2:
		index = count2 - 1;
		array = array2;
		max_count = MAXCOUNT2;
		break;
	   case DEPTH_A3:
		index = count3 - 1;
		array = array3;
		max_count = MAXCOUNT3;
		break;
	   default:
		return false;
	}


	//cout <<"depth.cpp: vehicle.curr_depth: "<<vehicle.curr_depth<<endl;
	//cout <<"depth.cpp: count1 : "<<count1<<endl;
	

	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;

}


/*
 * This is going to be called by commands.c where "result"
 * is this hist[]
 */
bool
Tel_Depth::get_head_hist(char hist[])
{
	int index;
	int max_count;
	int ntimes = 0;
	char   temp[128];
 	
 	
	index = headCount - 1;
	max_count = MAXCOUNT2;

	bzero(hist, strlen(hist));
	for (ntimes, index; index >= 0 && ntimes < 50; ++ntimes, --index) {  
		snprintf(temp, 128, "%4.1f|", headArray[index]);	
		strcat(hist, temp);
	} 
	max_count--;
	for(ntimes; ntimes < 50; ntimes++) {
		snprintf(temp, 128, "%4.1f|", headArray[max_count]);
		max_count--;
		strcat(hist, temp);
	}	

 	//cout <<"index : "<<index<<endl;
        //cout <<"hist : "<<hist<<endl;
 	
 	
   	return true;

}