/*
 File:		TelemetryState.java
 Compiler:	jdk1.3
 */

package demo.auv;

import java.util.*;

/**
 *  Manipulates telemetry files.  Needed:  major reconciliation of variable names.
 *<P>
 *
 *@version 0.7
 *<P>
 *
 *@author Don Brutzman (<A HREF="http://web.nps.navy.mil/~brutzman"><i>web.nps.navy.mil/~brutzman</i></A>) <A HREF="mailto:brutzman@nps.navy.mil(Don Brutzman)?subject=rra feedback "><i>brutzman@nps.navy.mil</i></A>
 *<P>
 *
 *<dt><b>Source Code:</b>
 *<dd><a href="../../../demo/auv/TelemetryState.java">TelemetryState.java</a>
 *<P>
 *
 *<dt><b>History:</b>
 *
 *<TABLE>
 *<tr>
 *	<td>	27 Nov 98
 *	<td>	Don Brutzman		
 *	<td>	Initial version
 *<tr>
 *	<td>	21 Jan 99
 *	<td>	Don Brutzman		
 *	<td>	Change packages and directories
 *</TABLE>
 *<P>
 *
 *@see TelemetryPlayback
 */

public class TelemetryState {

	static boolean DEBUG = false;

	public static void debug (String message)
	{
		if (DEBUG) System.out.println (message);
	}

/**
*  keyword identifier
*/
private String keyword;

/**
*  Timestamp
*/
private float t;

/**
 * 
 */
private float x;

/**
 * 
 */
private float y;

/**
 * depth
 */
private float z;

/**
 * estimated depth
 */
private float z_est;

/**
 * roll
 */
private float phi;

/**
 * pitch
 */
private float theta;

/**
 * yaw
 */
private float psi;

/**
 * depth rate
 */
private float x_dot;

/**
 * depth rate
 */
private float y_dot;

/**
 * depth rate
 */
private float z_dot;

/**
 * roll rate
 */
private float phi_dot;

/**
 * pitch rate
 */
private float theta_dot;

/**
 * yaw rate
 */
private float psi_dot;

/**
 * paddlewheel speed
 */
private float speed;

/**
 * surge
 */
private float u;

/**
 * sway
 */
private float v;

/**
 * heave
 */
private float w;

/**
 * roll rate
 */
private float p;

/**
 * pitch rate
 */
private float q;

/**
 * yaw rate
 */
private float r;


///////////////////////////////
/**
 * 
 */
private float delta_planes_bow;

/**
 * 
 */
private float delta_planes_stern;

/**
 * 
 */
private float delta_rudder;

/**
 * 
 */
private float port_rpm;

/**
 * 
 */
private float stbd_rpm;

/**
 * 
 */
private float AUV_bow_vertical;

/**
 * 
 */
private float AUV_stern_vertical;

/**
 * 
 */
private float AUV_bow_lateral;

/**
 * 
 */
private float AUV_stern_lateral;

/**
 * 
 */
private float AUV_ST1000_bearing;

/**
 * 
 */
private float AUV_ST1000_range;

/**
 * 
 */
private float AUV_ST1000_strength;

/**
 * 
 */
private float AUV_ST725_bearing;

/**
 * 
 */
private float AUV_ST725_range;

/**
 * 
 */
private float AUV_ST725_strength;

/**
 * 
 */
private float divetracker_range1;

/**
 * 
 */
private float divetracker_range2;

/**
 * 
 */
private float doppler_sog_u;

/**
 * 
 */
private float doppler_sog_v;

/**
 * 
 */
private float doppler_stw_u;

/**
 * 
 */
private float doppler_stw_v;

/**
 * 
 */
private float doppler_altitude;


////////////////////////



/**
 * rpm left screw
 */
private float n_ls;

/**
 * rpm right screw
 */
private float n_rs;

/**
 * voltage difference? left screw
 */
private float vd_ls;

/**
 *  voltage difference? right screw
 */
private float vd_rs;

/**
 * delta rudder?
 */
private float dr;

/**
 * delta stern plane? (bow planes are opposite)
 */
private float ds;

/**
 * Amps of current, left screw
 */
private float AmpereLs;

/**
 * Amps of current, right screw
 */
private float AmpereRs;

/**
 * 
 */
private float ADV_Id;

/**
 * Acoustic doppler velocity?, x component
 */
private float ADV_Vx;

/**
 * Acoustic doppler velocity?, y component
 */
private float ADV_Vy;

/**
 * Acoustic doppler velocity?, z component
 */
private float ADV_Vz;

/**
 * Acoustic doppler velocity?, roll
 */
private float ADV_Roll;

/**
 * Acoustic doppler velocity?, pitch
 */
private float ADV_Pitch;

/**
 * Acoustic doppler velocity?, heading
 */
private float ADV_Heading;

/**
 * x acceleration (u dot?)
 */
private float XAccel;

/**
 * y acceleration (v dot?)
 */
private float YAccel;

/**
 * z acceleration (w dot?)
 */
private float ZAccel;

/**
 * 
 */
private float DS30_Id;

/**
 * U over ground
 */
private float DS30_Ug;

/**
 * V over ground
 */
private float DS30_Vg;

/**
 * 
 */
private float DS30_Uf;

/**
 * 
 */
private float DS30_Vf;

/**
 * 
 */
private float DS30_Alt;

/**
 * Computer voltage
 */
private float CompVolt;

/**
 * Main motor voltage
 */
private float MotorVolt;

/**
 * 
 */
private float RDI_Id;

/**
 * 
 */
private float RDI_Ug;

/**
 * 
 */
private float RDI_Vg;

/**
 * 
 */
private float RDI_Wg;

/**
 * 
 */
private float RDI_Uf;

/**
 * 
 */
private float RDI_Vf;

/**
 * 
 */
private float RDI_Wf;

/**
 * 
 */
private float RDI_Alt;

/**
 * 
 */
private float Nav_Id;

/**
 * 
 */
private float Nav_X;

/**
 * 
 */
private float Nav_Y;

/**
 * 
 */
private float ADV_Amp1;

/**
 * 
 */
private float ADV_Amp2;

/**
 * 
 */
private float ADV_Amp3;

/**
 * 
 */
private float ADV_Cor1;

/**
 * 
 */
private float ADV_Cor2;

/**
 * 
 */
private float ADV_Cor3;

/**
*  No action needed for default constructor
*/

public TelemetryState () { } // 
	

public void  set_t (float value) { t = value; }
public float get_t ()            { return t; }

public void  set_x (float value) { x = value; }
public float get_x ()            { return x; }

public void  set_y (float value) { y = value; }
public float get_y ()            { return y; }

public void  set_z (float value) { z = value; }
public float get_z ()            { return z; }

public void  set_z_est (float value) { z_est = value; }
public float get_z_est ()            { return z_est; }

public void  set_phi (float value) { phi = value; }
public float get_phi ()            { return phi; }

public void  set_theta (float value) { theta = value; }
public float get_theta ()            { return theta; }

public void  set_psi (float value) { psi = value; }
public float get_psi ()            { return psi; }

public void  set_x_dot (float value) { x_dot = value; }
public float get_x_dot ()            { return x_dot; }

public void  set_y_dot (float value) { y_dot = value; }
public float get_y_dot ()            { return y_dot; }

public void  set_z_dot (float value) { z_dot = value; }
public float get_z_dot ()            { return z_dot; }

public void  set_phi_dot (float value) { phi_dot = value; }
public float get_phi_dot ()            { return phi_dot; }

public void  set_theta_dot (float value) { theta_dot = value; }
public float get_theta_dot ()            { return theta_dot; }

public void  set_psi_dot (float value) { psi_dot = value; }
public float get_psi_dot ()            { return psi_dot; }

public void  set_speed (float value) { speed = value; }
public float get_speed ()            { return speed; }

public void  set_u (float value) { u = value; }
public float get_u ()            { return u; }

public void  set_v (float value) { v = value; }
public float get_v ()            { return v; }

public void  set_w (float value) { w = value; }
public float get_w ()            { return w; }

public void  set_p (float value) { p = value; }
public float get_p ()            { return p; }

public void  set_q (float value) { q = value; }
public float get_q ()            { return q; }

public void  set_r (float value) { r = value; }
public float get_r ()            { return r; }

////////////

public void  set_delta_planes_bow (float value) { delta_planes_bow = value; }
public float get_delta_planes_bow ()            { return delta_planes_bow; }

public void  set_delta_planes_stern (float value) { delta_planes_stern = value; }
public float get_delta_planes_stern ()            { return delta_planes_stern; }

public void set_delta_rudder (float value) { delta_rudder = value; }
public float get_delta_rudder ()            { return delta_rudder; }

public void  set_port_rpm (float value) { port_rpm = value; }
public float get_port_rpm ()            { return port_rpm; }

public void  set_stbd_rpm (float value) { stbd_rpm = value; }
public float get_stbd_rpm ()            { return stbd_rpm; }

public void  set_AUV_bow_vertical (float value) { AUV_bow_vertical = value; }
public float get_AUV_bow_vertical ()            { return AUV_bow_vertical; }

public void  set_AUV_stern_vertical (float value) { AUV_stern_vertical = value; }
public float get_AUV_stern_vertical ()            { return AUV_stern_vertical; }

public void  set_AUV_bow_lateral (float value) { AUV_bow_lateral = value; }
public float get_AUV_bow_lateral ()            { return AUV_bow_lateral; }

public void  set_AUV_stern_lateral (float value) { AUV_stern_lateral = value; }
public float get_AUV_stern_lateral ()            { return AUV_stern_lateral; }

public void  set_AUV_ST1000_bearing (float value) { AUV_ST1000_bearing = value; }
public float get_AUV_ST1000_bearing ()            { return AUV_ST1000_bearing; }

public void  set_AUV_ST1000_range (float value) { AUV_ST1000_range = value; }
public float get_AUV_ST1000_range ()            { return AUV_ST1000_range; }

public void  set_AUV_ST1000_strength (float value) { AUV_ST1000_strength = value; }
public float get_AUV_ST1000_strength ()            { return AUV_ST1000_strength; }

public void  set_AUV_ST725_bearing (float value) { AUV_ST725_bearing = value; }
public float get_AUV_ST725_bearing ()            { return AUV_ST725_bearing; }

public void  set_AUV_ST725_range (float value) { AUV_ST725_range = value; }
public float get_AUV_ST725_range ()            { return AUV_ST725_range; }

public void  set_AUV_ST725_strength (float value) { AUV_ST725_strength = value; }
public float get_AUV_ST725_strength ()            { return AUV_ST725_strength; }

public void  set_divetracker_range1 (float value) { divetracker_range1 = value; }
public float get_divetracker_range1 ()            { return divetracker_range1; }

public void  set_divetracker_range2 (float value) { divetracker_range2 = value; }
public float get_divetracker_range2 ()            { return divetracker_range2; }

public void  set_doppler_sog_u (float value) { doppler_sog_u = value; }
public float get_doppler_sog_u ()            { return doppler_sog_u; }

public void  set_doppler_sog_v (float value) { doppler_sog_v = value; }
public float get_doppler_sog_v ()            { return doppler_sog_v; }

public void  set_doppler_stw_u (float value) { doppler_stw_u = value; }
public float get_doppler_stw_u ()            { return doppler_stw_u; }

public void  set_doppler_stw_v (float value) { doppler_stw_v = value; }
public float get_doppler_stw_v ()            { return doppler_stw_v; }

public void  set_n_ls (float value) { n_ls = value; }
public float get_n_ls()             { return n_ls; }

public void  set_n_rs (float value) { n_rs = value; }
public float get_n_rs ()            { return n_rs; }

public void  set_vd_ls (float value) { vd_ls = value; }
public float get_vd_ls ()            { return vd_ls; }

public void  set_vd_rs (float value) { vd_rs = value; }
public float get_vd_rs ()            { return vd_rs; }

public void  set_dr (float value) { dr = value; }
public float get_dr ()            { return dr; }

public void  set_ds (float value) { ds = value; }
public float get_ds ()            { return ds; }

public void  set_AmpereLs (float value) { AmpereLs = value; }
public float get_AmpereLs ()            { return AmpereLs; }

public void  set_AmpereRs (float value) { AmpereRs = value; }
public float get_AmpereRs ()            { return AmpereRs; }

public void  set_ADV_Id (float value) { ADV_Id = value; }
public float get_ADV_Id ()            { return ADV_Id; }

public void  set_ADV_Vx (float value) { ADV_Vx = value; }
public float get_ADV_Vx ()            { return ADV_Vx; }

public void  set_ADV_Vy (float value) { ADV_Vy = value; }
public float get_ADV_Vy ()            { return ADV_Vy; }

public void  set_ADV_Vz (float value) { ADV_Vz = value; }
public float get_ADV_Vz ()            { return ADV_Vz; }

public void  set_ADV_Roll (float value) { ADV_Roll = value; }
public float get_ADV_Roll ()            { return ADV_Roll; }

public void  set_ADV_Pitch (float value) { ADV_Pitch = value; }
public float get_ADV_Pitch ()            { return ADV_Pitch; }

public void  set_ADV_Heading (float value) { ADV_Heading = value; }
public float get_ADV_Heading ()            { return ADV_Heading; }

public void  set_XAccel (float value) { XAccel = value; }
public float get_XAccel ()            { return XAccel; }

public void  set_YAccel (float value) { YAccel = value; }
public float get_YAccel ()            { return YAccel; }

public void  set_ZAccel (float value) { ZAccel = value; }
public float get_ZAccel ()            { return ZAccel; }

public void  set_DS30_Id (float value) { DS30_Id = value; }
public float get_DS30_Id ()            { return DS30_Id; }

public void  set_DS30_Ug (float value) { DS30_Ug = value; }
public float get_DS30_Ug ()            { return DS30_Ug; }

public void  set_DS30_Vg (float value) { DS30_Vg = value; }
public float get_DS30_Vg ()            { return DS30_Vg; }

public void  set_DS30_Uf (float value) { DS30_Uf = value; }
public float get_DS30_Uf ()            { return DS30_Uf; }

public void  set_DS30_Vf (float value) { DS30_Vf = value; }
public float get_DS30_Vf ()            { return DS30_Vf; }

public void  set_DS30_Alt (float value) { DS30_Alt = value; }
public float get_DS30_Alt ()            { return DS30_Alt; }

public void  set_CompVolt (float value) { CompVolt = value; }
public float get_CompVolt ()            { return CompVolt; }

public void  set_MotorVolt (float value) { MotorVolt = value; }
public float get_MotorVolt ()            { return MotorVolt; }

public void  set_RDI_Id (float value) { RDI_Id = value; }
public float get_RDI_Id ()            { return RDI_Id; }

public void  set_RDI_Ug (float value) { RDI_Ug = value; }
public float get_RDI_Ug ()            { return RDI_Ug; }

public void  set_RDI_Vg (float value) { RDI_Vg = value; }
public float get_RDI_Vg ()            { return RDI_Vg; }

public void  set_RDI_Wg (float value) { RDI_Wg = value; }
public float get_RDI_Wg ()            { return RDI_Wg; }

public void  set_RDI_Uf (float value) { RDI_Uf = value; }
public float get_RDI_Uf ()            { return RDI_Uf; }

public void  set_RDI_Vf (float value) { RDI_Vf = value; }
public float get_RDI_Vf ()            { return RDI_Vf; }

public void  set_RDI_Wf (float value) { RDI_Wf = value; }
public float get_RDI_Wf ()            { return RDI_Wf; }

public void  set_RDI_Alt (float value) { RDI_Alt = value; }
public float get_RDI_Alt ()            { return RDI_Alt; }

public void  set_Nav_Id (float value) { Nav_Id = value; }
public float get_Nav_Id ()            { return Nav_Id; }

public void  set_Nav_X (float value) { Nav_X = value; }
public float get_Nav_X ()            { return Nav_X; }

public void  set_Nav_Y (float value) { Nav_Y = value; }
public float get_Nav_Y ()            { return Nav_Y; }

public void  set_ADV_Amp1 (float value) { ADV_Amp1 = value; }
public float get_ADV_Amp1 ()            { return ADV_Amp1; }

public void  set_ADV_Amp2 (float value) { ADV_Amp2 = value; }
public float get_ADV_Amp2 ()            { return ADV_Amp2; }

public void  set_ADV_Amp3 (float value) { ADV_Amp3 = value; }
public float get_ADV_Amp3 ()            { return ADV_Amp3; }

public void  set_ADV_Cor1 (float value) { ADV_Cor1 = value; }
public float get_ADV_Cor1 ()            { return ADV_Cor1; }

public void  set_ADV_Cor2 (float value) { ADV_Cor2 = value; }
public float get_ADV_Cor2 ()            { return ADV_Cor2; }

public void  set_ADV_Cor3 (float value) { ADV_Cor3 = value; }
public float get_ADV_Cor3 ()            { return ADV_Cor3; }

////////////////////

public boolean update (String newTelemetryLine)
{
	StringTokenizer tokens = new StringTokenizer( newTelemetryLine );
	int elementCount       = tokens.countTokens();

if (elementCount == 37) // auv uvw mission.output.telemetry
	{
		keyword = new String(tokens.nextToken());
		if (keyword.charAt(0) == '#') return false; // comment
		set_t (Float.parseFloat(tokens.nextToken()));
		set_x (Float.parseFloat(tokens.nextToken()));
		set_y (Float.parseFloat(tokens.nextToken()));
		set_z (Float.parseFloat(tokens.nextToken()));
		set_phi (Float.parseFloat(tokens.nextToken()));
		set_theta (Float.parseFloat(tokens.nextToken()));
		set_psi (Float.parseFloat(tokens.nextToken()));
		set_speed (Float.parseFloat(tokens.nextToken()));
		set_u (Float.parseFloat(tokens.nextToken()));
		set_v (Float.parseFloat(tokens.nextToken()));
		set_w (Float.parseFloat(tokens.nextToken()));
		set_p (Float.parseFloat(tokens.nextToken()));
		set_q (Float.parseFloat(tokens.nextToken()));
		set_r (Float.parseFloat(tokens.nextToken()));
		set_x_dot (Float.parseFloat(tokens.nextToken()));
		set_y_dot (Float.parseFloat(tokens.nextToken()));
		set_z_dot (Float.parseFloat(tokens.nextToken()));
		set_phi_dot (Float.parseFloat(tokens.nextToken()));
		set_theta_dot (Float.parseFloat(tokens.nextToken()));
		set_psi_dot (Float.parseFloat(tokens.nextToken()));
		set_delta_rudder (Float.parseFloat(tokens.nextToken()));
		set_delta_planes_bow (Float.parseFloat(tokens.nextToken()));
		set_delta_planes_stern (delta_planes_bow);
		set_port_rpm (Float.parseFloat(tokens.nextToken()));
		set_stbd_rpm (Float.parseFloat(tokens.nextToken()));
		set_AUV_bow_vertical (Float.parseFloat(tokens.nextToken()));
		set_AUV_stern_vertical (Float.parseFloat(tokens.nextToken()));
		set_AUV_bow_lateral (Float.parseFloat(tokens.nextToken()));
		set_AUV_stern_lateral (Float.parseFloat(tokens.nextToken()));
		set_AUV_ST1000_bearing (Float.parseFloat(tokens.nextToken()));
		set_AUV_ST1000_range (Float.parseFloat(tokens.nextToken()));
		set_AUV_ST1000_strength (Float.parseFloat(tokens.nextToken()));
		set_AUV_ST725_bearing (Float.parseFloat(tokens.nextToken()));
		set_AUV_ST725_range (Float.parseFloat(tokens.nextToken()));
		set_AUV_ST725_strength (Float.parseFloat(tokens.nextToken()));
		set_divetracker_range1 (Float.parseFloat(tokens.nextToken()));
		set_divetracker_range2 (Float.parseFloat(tokens.nextToken()));

		debug ("t=" + get_t() + ", x=" + get_x ()
		                       + ", y=" + get_y ()  + ", z=" + get_z ());
	}
	else if (elementCount == 45)  // Gulf of Mexico telemetry.d
	{
		set_t (Float.parseFloat(tokens.nextToken()));
		set_z_est (Float.parseFloat(tokens.nextToken()));
		set_z_dot (Float.parseFloat(tokens.nextToken()));
		set_p (Float.parseFloat(tokens.nextToken()));
		set_theta (Float.parseFloat(tokens.nextToken()));
		set_q (Float.parseFloat(tokens.nextToken()));
		set_psi (Float.parseFloat(tokens.nextToken()));
		set_r (Float.parseFloat(tokens.nextToken()));
		set_n_ls (Float.parseFloat(tokens.nextToken()));
		set_n_rs (Float.parseFloat(tokens.nextToken()));
		set_vd_ls (Float.parseFloat(tokens.nextToken()));
		set_vd_rs (Float.parseFloat(tokens.nextToken()));
		set_dr (Float.parseFloat(tokens.nextToken()));
		set_ds (Float.parseFloat(tokens.nextToken()));
		set_AmpereLs (Float.parseFloat(tokens.nextToken()));
		set_AmpereRs (Float.parseFloat(tokens.nextToken()));
		set_ADV_Id (Float.parseFloat(tokens.nextToken()));
		set_ADV_Vx (Float.parseFloat(tokens.nextToken()));
		set_ADV_Vy (Float.parseFloat(tokens.nextToken()));
		set_ADV_Vz (Float.parseFloat(tokens.nextToken()));
		set_ADV_Roll (Float.parseFloat(tokens.nextToken()));
		set_ADV_Pitch (Float.parseFloat(tokens.nextToken()));
		set_ADV_Heading (Float.parseFloat(tokens.nextToken()));
		set_XAccel (Float.parseFloat(tokens.nextToken()));
		set_YAccel (Float.parseFloat(tokens.nextToken()));
		set_ZAccel (Float.parseFloat(tokens.nextToken()));
//		set_DS30_Id (Float.parseFloat(tokens.nextToken()));
//		set_DS30_Ug (Float.parseFloat(tokens.nextToken()));
//		set_DS30_Vg (Float.parseFloat(tokens.nextToken()));
//		set_DS30_Uf (Float.parseFloat(tokens.nextToken()));
//		set_DS30_Vf (Float.parseFloat(tokens.nextToken()));
//		set_DS30_Alt (Float.parseFloat(tokens.nextToken()));
		set_RDI_Id (Float.parseFloat(tokens.nextToken()));
		set_RDI_Ug (Float.parseFloat(tokens.nextToken()));
		set_RDI_Vg (Float.parseFloat(tokens.nextToken()));
		set_RDI_Wg (Float.parseFloat(tokens.nextToken()));
		set_RDI_Uf (Float.parseFloat(tokens.nextToken()));
		set_RDI_Vf (Float.parseFloat(tokens.nextToken()));
		set_RDI_Wf (Float.parseFloat(tokens.nextToken()));
		set_RDI_Alt (Float.parseFloat(tokens.nextToken()));
		set_Nav_Id (Float.parseFloat(tokens.nextToken()));
		set_Nav_X (Float.parseFloat(tokens.nextToken()));
		set_Nav_Y (Float.parseFloat(tokens.nextToken()));
		set_CompVolt (Float.parseFloat(tokens.nextToken()));
		set_MotorVolt (Float.parseFloat(tokens.nextToken()));
		set_ADV_Amp1 (Float.parseFloat(tokens.nextToken()));
		set_ADV_Amp2 (Float.parseFloat(tokens.nextToken()));
		set_ADV_Amp3 (Float.parseFloat(tokens.nextToken()));
		set_ADV_Cor1 (Float.parseFloat(tokens.nextToken()));
		set_ADV_Cor2 (Float.parseFloat(tokens.nextToken()));
		set_ADV_Cor3 (Float.parseFloat(tokens.nextToken()));
/*
From:    marco@me.nps.navy.mil (David Marco)        Mon 9:26 AM, 23 NOV 98
Subject: Re: telemetry file problems


 n_ls, n_rs [=] Rotation/Sec
 z_est (depth) Feet
 All other linear units are meters
 Also we no longer have the speed wheel.

  David Marco, PhD.
*/

// convert to state variables used by virtual world / VRML interpolators
//		set_t ();
		set_x (Nav_X * 3.275f);  // convert meters to feet
		set_y (Nav_Y * 3.275f);  // convert meters to feet
		set_z (z_est);          // already in feet
		set_phi   (ADV_Roll     * 180.0f / 3.141592653f); // convert radians to degrees
//		set_theta (ADV_Pitch    * 180.0f / 3.141592653f); // note theta already recorded
		set_theta (get_theta () * 180.0f / 3.141592653f); // convert radians to degrees
//		set_psi   (ADV_Heading  * 180.0f / 3.141592653f); // note psi   already recorded
		set_psi   (get_psi   () * 180.0f / 3.141592653f); // convert radians to degrees
//		set_speed ();
		set_u (RDI_Ug * 3.275f);  // convert meters to feet
		set_v (RDI_Vg * 3.275f);  // convert meters to feet
		set_w (RDI_Wf * 3.275f);  // convert meters to feet
		set_p (get_p () * 180.0f / 3.141592653f); // convert radians/sec to degrees/sec
		set_q (get_q () * 180.0f / 3.141592653f); // convert radians/sec to degrees/sec
		set_r (get_r () * 180.0f / 3.141592653f); // convert radians/sec to degrees/sec
//		set_x_dot ();
//		set_y_dot ();
//		set_z_dot ();
//		set_phi_dot ();
//		set_theta_dot ();
//		set_psi_dot ();
		set_delta_rudder     (dr * 180.0f / 3.141592653f); // convert radians to degrees
		set_delta_planes_bow (ds * 180.0f / 3.141592653f); // convert radians to degrees
		set_delta_planes_stern (get_delta_planes_bow ());
		set_port_rpm (n_ls * 60.0f);  // convert revolutions/second to rpm
		set_stbd_rpm (n_rs * 60.0f);  // convert revolutions/second to rpm
//		set_AUV_bow_vertical ();
//		set_AUV_stern_vertical ();
//		set_AUV_bow_lateral ();
//		set_AUV_stern_lateral ();
//		set_AUV_ST1000_bearing ();
//		set_AUV_ST1000_range ();
//		set_AUV_ST1000_strength ();
//		set_AUV_ST725_bearing ();
//		set_AUV_ST725_range ();
//		set_AUV_ST725_strength ();
//		set_divetracker_range1 ();
//		set_divetracker_range2 ();
	}
	else
	{
		System.out.println ("invalid string, only " + elementCount + " elements");
		return false;
	}
	
//	if ( tokens.hasMoreTokens() )
//		debug ( tokens.countTokens() + " unused tokens remaining" );
	
/*
RDI_Id
RDI_Ug
RDI_Vg
RDI_Wg
RDI_Uf
RDI_Vf
RDI_Wf
RDI_Alt
Nav_Id
Nav_X
Nav_Y
ADV_Amp1
ADV_Amp2
ADV_Amp3
ADV_Cor1
ADV_Cor2
ADV_Cor3
*/



	return true;
}

public void update (TelemetryState newTelemetryState)
{
	set_t (newTelemetryState.get_t());
	set_x (newTelemetryState.get_x());
	set_y (newTelemetryState.get_y());
	set_z (newTelemetryState.get_z());
	set_z_est (newTelemetryState.get_z_est());
	set_phi (newTelemetryState.get_phi());
	set_theta (newTelemetryState.get_theta());
	set_psi (newTelemetryState.get_psi());
	set_speed (newTelemetryState.get_speed());
	set_u (newTelemetryState.get_u());
	set_v (newTelemetryState.get_v());
	set_w (newTelemetryState.get_w());
	set_p (newTelemetryState.get_p());
	set_q (newTelemetryState.get_q());
	set_r (newTelemetryState.get_r());
	set_x_dot (newTelemetryState.get_x_dot());
	set_y_dot (newTelemetryState.get_y_dot());
	set_z_dot (newTelemetryState.get_z_dot());
	set_phi_dot (newTelemetryState.get_phi_dot());
	set_theta_dot (newTelemetryState.get_theta_dot());
	set_psi_dot (newTelemetryState.get_psi_dot());
	set_delta_rudder (newTelemetryState.get_delta_rudder());
	set_delta_planes_bow (newTelemetryState.get_delta_planes_bow());
	set_delta_planes_stern (delta_planes_bow);
	set_port_rpm (newTelemetryState.get_port_rpm());
	set_stbd_rpm (newTelemetryState.get_stbd_rpm());
	
	set_AUV_bow_vertical (newTelemetryState.get_AUV_bow_vertical());
	set_AUV_stern_vertical (newTelemetryState.get_AUV_stern_vertical());
	set_AUV_bow_lateral (newTelemetryState.get_AUV_bow_lateral());
	set_AUV_stern_lateral (newTelemetryState.get_AUV_stern_lateral());
	set_AUV_ST725_bearing (newTelemetryState.get_AUV_ST725_bearing());
	set_AUV_ST725_range (newTelemetryState.get_AUV_ST725_range());
	set_AUV_ST725_strength (newTelemetryState.get_AUV_ST725_strength());
	set_AUV_ST1000_bearing (newTelemetryState.get_AUV_ST1000_bearing());
	set_AUV_ST1000_range (newTelemetryState.get_AUV_ST1000_range());
	set_AUV_ST1000_strength (newTelemetryState.get_AUV_ST1000_strength());

	set_n_ls (newTelemetryState.get_n_ls());
	set_n_rs (newTelemetryState.get_n_rs());
	set_vd_ls (newTelemetryState.get_vd_ls());
	set_vd_rs (newTelemetryState.get_vd_rs());
	set_dr (newTelemetryState.get_dr());
	set_ds (newTelemetryState.get_ds());
	set_AmpereLs (newTelemetryState.get_AmpereLs());
	set_AmpereRs (newTelemetryState.get_AmpereRs());
	set_ADV_Id (newTelemetryState.get_ADV_Id());
	set_ADV_Vx (newTelemetryState.get_ADV_Vx());
	set_ADV_Vy (newTelemetryState.get_ADV_Vy());
	set_ADV_Vz (newTelemetryState.get_ADV_Vz());
	set_ADV_Roll (newTelemetryState.get_ADV_Roll());
	set_ADV_Pitch (newTelemetryState.get_ADV_Pitch());
	set_ADV_Heading (newTelemetryState.get_ADV_Heading());
	set_XAccel (newTelemetryState.get_XAccel());
	set_YAccel (newTelemetryState.get_YAccel());
	set_ZAccel (newTelemetryState.get_ZAccel());
	set_DS30_Id (newTelemetryState.get_DS30_Id());
	set_DS30_Ug (newTelemetryState.get_DS30_Ug());
	set_DS30_Vg (newTelemetryState.get_DS30_Vg());
	set_DS30_Uf (newTelemetryState.get_DS30_Uf());
	set_DS30_Vf (newTelemetryState.get_DS30_Vf());
	set_DS30_Alt (newTelemetryState.get_DS30_Alt());
	set_CompVolt (newTelemetryState.get_CompVolt());
	set_MotorVolt (newTelemetryState.get_MotorVolt());
}


/*
t
z_est
z_dot
p
theta
q
psi
r
n_ls
n_rs
vd_ls
vd_rs
dr
ds
AmpereLs
AmpereRs
ADV_Id
ADV_Vx
ADV_Vy
ADV_Vz
ADV_Roll
ADV_Pitch
ADV_Heading
XAccel
YAccel
ZAccel
DS30_Id
DS30_Ug
DS30_Vg
DS30_Uf
DS30_Vf
DS30_Alt
CompVolt
MotorVolt
*/

}