#include "t2_struct.h"
#include "control_struct.h"
#include <stdio.h>
#define GET_PH_DEBUG 0

struct p_st get_p(int option) {
//struct p_st get_p(void) {
double test; 	
struct p_st p ;
	int i ;
	FILE *fopen(), *fin ;
	
//	printf("get_p got option:  %d\n", option) ;
	if (option < 3)
	  fin = fopen("mission_p","r") ;
	else 
	  fin = fopen("mission_p2","r") ;
//	fin = fopen("mission_p2","r"); 

//-----------------------------------------------------------------
// control_st.h stuff -----------------------------------------
//-----------------------------------------------------------------
	
	fscanf(fin,"%*[^:]: %d", &p.ol.STEP) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.ol.amp[0], &p.ol.amp[1], &p.ol.amp[2]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.ol.t[0], &p.ol.t[1], &p.ol.t[2]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.ol.freq[0], &p.ol.freq[1], &p.ol.freq[2]) ;
	
	for (i=0;i<MAX_OPTIONS;i++)
		fscanf(fin,"%*[^:]: %lf %lf %lf", &p.pose.enc[i][0], 
			&p.pose.enc[i][1], &p.pose.enc[i][2]) ;
	fscanf(fin,"%*[^:]: %lf", &p.pose.t) ;

	for (i=0;i<MAX_OPTIONS;i++) {	
		fscanf(fin,"%*[^:]: %d", &p.move.CARTESIAN[i]) ;
		fscanf(fin,"%*[^:]: %lf %lf %lf", &p.move.amp[i][0],   
			&p.move.amp[i][1],   &p.move.amp[i][2]) ;
		fscanf(fin,"%*[^:]: %lf %lf %lf", &p.move.freq[i][0],  
			&p.move.freq[i][1],  &p.move.freq[i][2]) ;
		fscanf(fin,"%*[^:]: %lf %lf %lf", &p.move.phase[i][0], 
			&p.move.phase[i][1], &p.move.phase[i][2]) ;
    }
	fscanf(fin,"%*[^:]: %lf", &p.move.env_tau) ;
	
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.dither_amp[0], &p.servo.dither_amp[1], 
		&p.servo.dither_amp[2]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.dither_freq[0], &p.servo.dither_freq[1], 
		&p.servo.dither_freq[2]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.kp[0], &p.servo.kp[1], &p.servo.kp[2]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.kd[0], &p.servo.kd[1], &p.servo.kd[2]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.kdd[0], &p.servo.kdd[1], &p.servo.kdd[2]) ;
	for (i=0;i<MAX_OPTIONS;i++) {
		fscanf(fin,"%*[^:]: %lf", &p.servo.panto[i]) ;
		p.servo.panto[i] /= 2.5 ; /* mod FSH Feb 12 1998 */
    }
	fscanf(fin,"%*[^:]: %lf", &p.servo.pantod) ;

	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.vel_ffwd[0], &p.servo.vel_ffwd[1], 
		&p.servo.vel_ffwd[2]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.max_speed[0], &p.servo.max_speed[1], 
		&p.servo.max_speed[2]) ;
	for (i=0;i<MAX_OPTIONS;i++)
		fscanf(fin,"%*[^:]: %lf %lf %lf", &p.servo.tau[i][0], 
			&p.servo.tau[i][1], &p.servo.tau[i][2]) ;
	fscanf(fin,"%*[^:]: %lf", &p.servo.max_spool) ;
	
	fscanf(fin,"%*[^:]: %lf %lf %lf", &p.robot.l[0], &p.robot.l[1], &p.robot.l[2]) ;
	fscanf(fin,"%*[^:]: %lf", &p.robot.sign_s2) ;
	fscanf(fin,"%*[^:]: %d", &p.robot.ii) ;
	
	fscanf(fin,"%*[^:]: %d", &p.DEBUG) ;
	fscanf(fin,"%*[^:]: %lf", &test) ;

	fclose(fin) ;

	if (fabs(test - 3.14159) > 0.01)
	{ 
		printf("Improper number of values in mission_p file\n");
		printf("    or file missing.\n");
		exit(0);
	}
	else
	{
		printf("MISSION_P data check passed test.\n");	
	}

	if (GET_PH_DEBUG) {
		printf("\nSTRUCTURE LOAD DIAGNOSTICS:\n");
		printf("p.ol.STEP: %d\n", p.ol.STEP); 
		printf("p.pose.t:  %lf\n", p.pose.t) ;
		printf("p.servo.kp[1]:  %lf\n", p.servo.kp[1]) ;
	}

	fclose(fin) ;
	return(p) ;
}	
	
/*-------------------------------------------------------*/

struct h_st get_h(void) {

double test ;
struct h_st h ;
FILE *fopen(), *fin ;

fin = fopen("mission_h","r");

	fscanf(fin,"%*[^:]: %lf", &h.io.stop_time) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf %lf", &h.io.act_sign.hip, &h.io.act_sign.knee,
		&h.io.act_sign.foot, &h.io.act_sign.toe) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf %lf", &h.io.act_offset.hip, &h.io.act_offset.knee,
		&h.io.act_offset.foot, &h.io.act_offset.toe) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf %lf", &h.io.enc_sign.hip, &h.io.enc_sign.knee,
		&h.io.enc_sign.foot, &h.io.enc_sign.toe) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf %lf", &h.io.enc_offset.hip, &h.io.enc_offset.knee,
		&h.io.enc_offset.foot, &h.io.enc_offset.toe) ;
	fscanf(fin,"%*[^:]: %d", &h.io.MONITOR) ;
	
	fscanf(fin,"%*[^:]: %lf", &h.ad.gain) ;
	fscanf(fin,"%*[^:]: %lf", &h.ad.rate) ;
	fscanf(fin,"%*[^:]: %d", &h.ad.first_chan) ;
	fscanf(fin,"%*[^:]: %d", &h.ad.num_chan) ;
	fscanf(fin,"%*[^:]: %lf", &h.ad.burst_rate) ;
	
	fscanf(fin,"%*[^:]: %lf %lf %lf %lf", &h.limit.max_enc[0],&h.limit.max_enc[1], 
		&h.limit.max_enc[2],&h.limit.max_enc[3]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf %lf", &h.limit.min_enc[0],&h.limit.min_enc[1], 
		&h.limit.min_enc[2],&h.limit.min_enc[3]) ;
	fscanf(fin,"%*[^:]: %lf %lf %lf %lf", &h.limit.max_enc_rate[0],&h.limit.max_enc_rate[1], 
		&h.limit.max_enc_rate[2],&h.limit.max_enc_rate[3]) ;
	fscanf(fin,"%*[^:]: %d", &h.limit.emergency_channel) ;
	fscanf(fin,"%*[^:]: %lf", &h.limit.emergency_minV) ;

	fscanf(fin,"%*[^:]: %d", &h.logger.rows) ;
	fscanf(fin,"%*[^:]: %d", &h.logger.cols) ;
	fscanf(fin,"%*[^:]: %d", &h.logger.write_skip) ;
	fscanf(fin,"%*[^:]: %d", &h.logger.pressure_chan) ;
	fscanf(fin,"%*[^:]: %d", &h.logger.bailuff_chan) ;
	fscanf(fin,"%*[^:]: %d", &h.logger.accel_chan) ;
	
//---------------------------------------------------------

	fscanf(fin,"%*[^:]: %lf", &test) ;
	fclose(fin) ;

	if (fabs(test - 3.14159) > 0.01)
	{ 
		printf("Improper number of values in mission_h file\n");
		printf("    or file missing.\n");
		exit(0);
	}
	else
	{
		printf("MISSION_H data check passed test.\n");	
	}

	if(GET_PH_DEBUG){
		printf("h.io.stop_time:  %lf\n", h.io.stop_time) ;
		printf("h.io.MONITOR:  %d\n", h.io.MONITOR) ;
		printf("h.ad.num_chan:  %d\n", h.ad.num_chan); 
		printf("h.limit.emergency_minV:  %lf\n", 
				h.limit.emergency_minV) ;	
		printf("h.logger.accel_chan:  %d\n\n", h.logger.accel_chan) ;
	}

	fclose(fin) ;
	return(h); 
}
