/**
 **
 ** Imx5.java - InertialSense IMX-5 IMU/GPS binary log reader and parser
 **
 ** Copyright (C) 2023-2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  Imx5.java is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ** -----------------------------------------------------------------------------**
 **
 */
package com.elphel.imagej.ims;

//import java.io.ByteArrayOutputStream;
//import java.io.IOException;
//import java.io.ObjectOutputStream;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;

import com.elphel.imagej.tileprocessor.ErsCorrection;


public class Imx5 {
	
	
	// defines adapted from Inertial Sense data_sets.h 
	public static final int DID_NULL                        = 0 ; /** NULL (INVALID) */
	public static final int DID_DEV_INFO                    = 1 ; /** (dev_info_t) Device information */
	public static final int DID_SYS_FAULT                   = 2 ; /** (system_fault_t) System fault information */
	public static final int DID_PIMU                        = 3 ; /** (pimu_t) Preintegrated IMU (a.k.a. Coning and Sculling integral) in body/IMU frame.  Updated at IMU rate. Also know as delta theta delta velocity, or preintegrated IMU (PIMU). For clarification, the name "Preintegrated IMU" or "PIMU" throughout our User Manual. This data is integrated from the IMU data at the IMU update rate (startupImuDtMs, default 1ms).  The integration period (dt) and output data rate are the same as the NAV rate (startupNavDtMs) and cannot be output at any other rate. If a faster output data rate is desired, DID_IMU_RAW can be used instead. PIMU data acts as a form of compression, adding the benefit of higher integration rates for slower output data rates, preserving the IMU data without adding filter delay and addresses antialiasing. It is most effective for systems that have higher dynamics and lower communications data rates.  The minimum data period is DID_FLASH_CONFIG.startupImuDtMs or 4, whichever is larger (250Hz max). The PIMU value can be converted to IMU by dividing PIMU by dt (i.e. IMU = PIMU / dt)  */
	public static final int DID_INS_1                       = 4 ; /** (ins_1_t) INS output: euler rotation w/ respect to NED, NED position from reference LLA. */
	public static final int DID_INS_2                       = 5 ; /** (ins_2_t) INS output: quaternion rotation w/ respect to NED, ellipsoid altitude */
	public static final int DID_GPS1_UBX_POS                = 6 ; /** (gps_pos_t) GPS 1 position data from ublox receiver. */
	public static final int DID_SYS_CMD                     = 7 ; /** (system_command_t) System commands. Both the command and invCommand fields must be set at the same time for a command to take effect. */
	public static final int DID_ASCII_BCAST_PERIOD          = 8 ; /** (ascii_msgs_t) Broadcast period for ASCII messages */
	public static final int DID_RMC                         = 9 ; /** (rmc_t) Realtime Message Controller (RMC). The data sets available through RMC are driven by the availability of the data. The RMC provides updates from various data sources (i.e. sensors) as soon as possible with minimal latency. Several of the data sources (sensors) output data at different data rates that do not all correspond. The RMC is provided so that broadcast of sensor data is done as soon as it becomes available. All RMC messages can be enabled using the standard Get Data packet format. */
	public static final int DID_SYS_PARAMS                  = 10; /** (sys_params_t) System parameters / info */
	public static final int DID_SYS_SENSORS                 = 11; /** (sys_sensors_t) System sensor information */
	public static final int DID_FLASH_CONFIG                = 12; /** (nvm_flash_cfg_t) Flash memory configuration */
	public static final int DID_GPS1_POS                    = 13; /** (gps_pos_t) GPS 1 position data.  This comes from DID_GPS1_UBX_POS or DID_GPS1_RTK_POS, depending on whichever is more accurate. */
	public static final int DID_GPS2_POS                    = 14; /** (gps_pos_t) GPS 2 position data */
	public static final int DID_GPS1_SAT                    = 15; /** (gps_sat_t) GPS 1 GNSS and sat identifiers, carrier to noise ratio (signal strength), elevation and azimuth angles, pseudo range residual. */
	public static final int DID_GPS2_SAT                    = 16; /** (gps_sat_t) GPS 2 GNSS and sat identifiers, carrier to noise ratio (signal strength), elevation and azimuth angles, pseudo range residual. */
	public static final int DID_GPS1_VERSION                = 17; /** (gps_version_t) GPS 1 version info */
	public static final int DID_GPS2_VERSION                = 18; /** (gps_version_t) GPS 2 version info */
	public static final int DID_MAG_CAL                     = 19; /** (mag_cal_t) Magnetometer calibration */
	public static final int DID_INTERNAL_DIAGNOSTIC         = 20; /** INTERNAL USE ONLY (internal_diagnostic_t) Internal diagnostic info */
	public static final int DID_GPS1_RTK_POS_REL            = 21; /** (gps_rtk_rel_t) RTK precision position base to rover relative info. */
	public static final int DID_GPS1_RTK_POS_MISC           = 22; /** (gps_rtk_misc_t) RTK precision position related data. */
	public static final int DID_FEATURE_BITS                = 23; /** INTERNAL USE ONLY (feature_bits_t) */
	public static final int DID_SENSORS_UCAL                = 24; /** INTERNAL USE ONLY (sensors_w_temp_t) Uncalibrated IMU output. */
	public static final int DID_SENSORS_TCAL                = 25; /** INTERNAL USE ONLY (sensors_w_temp_t) Temperature compensated IMU output. */
	public static final int DID_SENSORS_TC_BIAS             = 26; /** INTERNAL USE ONLY (sensors_t) */
	public static final int DID_IO                          = 27; /** (io_t) I/O */
	public static final int DID_SENSORS_ADC                 = 28; /** INTERNAL USE ONLY (sys_sensors_adc_t) */
	public static final int DID_SCOMP                       = 29; /** INTERNAL USE ONLY (sensor_compensation_t) */
	public static final int DID_GPS1_VEL                    = 30; /** (gps_vel_t) GPS 1 velocity data */
	public static final int DID_GPS2_VEL                    = 31; /** (gps_vel_t) GPS 2 velocity data */
	public static final int DID_HDW_PARAMS                  = 32; /** INTERNAL USE ONLY (hdw_params_t) */
	public static final int DID_NVR_MANAGE_USERPAGE         = 33; /** INTERNAL USE ONLY (nvr_manage_t) */
	public static final int DID_NVR_USERPAGE_SN             = 34; /** INTERNAL USE ONLY (nvm_group_sn_t) */
	public static final int DID_NVR_USERPAGE_G0             = 35; /** INTERNAL USE ONLY (nvm_group_0_t) */
	public static final int DID_NVR_USERPAGE_G1             = 36; /** INTERNAL USE ONLY (nvm_group_1_t) */
	public static final int DID_DEBUG_STRING                = 37; /** INTERNAL USE ONLY (debug_string_t) */
	public static final int DID_RTOS_INFO                   = 38; /** (rtos_info_t) RTOS information. */
	public static final int DID_DEBUG_ARRAY                 = 39; /** INTERNAL USE ONLY (debug_array_t) */
	public static final int DID_SENSORS_MCAL                = 40; /** INTERNAL USE ONLY (sensors_w_temp_t) Temperature compensated and motion calibrated IMU output. */
	public static final int DID_GPS1_TIMEPULSE              = 41; /** INTERNAL USE ONLY (gps_timepulse_t) */
	public static final int DID_CAL_SC                      = 42; /** INTERNAL USE ONLY (sensor_cal_t) */
	public static final int DID_CAL_TEMP_COMP               = 43; /** INTERNAL USE ONLY (sensor_tcal_group_t) */
	public static final int DID_CAL_MOTION                  = 44; /** INTERNAL USE ONLY (sensor_mcal_group_t) */
	public static final int DID_UNUSED_45           		= 45; /** used to be internal DID_SYS_SENSORS_SIGMA */
	public static final int DID_SENSORS_ADC_SIGMA           = 46; /** INTERNAL USE ONLY (sys_sensors_adc_t) */
	public static final int DID_REFERENCE_MAGNETOMETER      = 47; /** (magnetometer_t) Reference or truth magnetometer used for manufacturing calibration and testing */
	public static final int DID_INL2_STATES                 = 48; /** (inl2_states_t) */
	public static final int DID_INL2_COVARIANCE_LD          = 49; /** (INL2_COVARIANCE_LD_ARRAY_SIZE) */
	public static final int DID_INL2_STATUS                 = 50; /** (inl2_status_t) */
	public static final int DID_INL2_MISC                   = 51; /** (inl2_misc_t) */
	public static final int DID_MAGNETOMETER                = 52; /** (magnetometer_t) Magnetometer sensor output */
	public static final int DID_BAROMETER                   = 53; /** (barometer_t) Barometric pressure sensor data */
	public static final int DID_GPS1_RTK_POS                = 54; /** (gps_pos_t) GPS RTK position data */
	public static final int DID_ROS_COVARIANCE_POSE_TWIST   = 55; /** (ros_covariance_pose_twist_t) INL2 EKF covariances matrix lower diagonals */
	public static final int DID_COMMUNICATIONS_LOOPBACK     = 56; /** INTERNAL USE ONLY - Unit test for communications manager  */
	public static final int DID_IMU3_UNCAL                  = 57; /** INTERNAL USE ONLY (imu3_t) Uncalibrated triple IMU data.  We recommend use of DID_IMU or DID_PIMU as they are calibrated and oversampled and contain less noise.  Minimum data period is DID_FLASH_CONFIG.startupImuDtMs or 4, whichever is larger (250Hz max). */
	public static final int DID_IMU                         = 58; /** (imu_t) Inertial measurement unit data down-sampled from IMU rate (DID_FLASH_CONFIG.startupImuDtMs (1KHz)) to navigation update rate (DID_FLASH_CONFIG.startupNavDtMs) as an anti-aliasing filter to reduce noise and preserve accuracy.  Minimum data period is DID_FLASH_CONFIG.startupNavDtMs (1KHz max).  */
	public static final int DID_INL2_MAG_OBS_INFO           = 59; /** (inl2_mag_obs_info_t) INL2 magnetometer calibration information. */
	public static final int DID_GPS_BASE_RAW                = 60; /** (gps_raw_t) GPS raw data for base station (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
	public static final int DID_GPS_RTK_OPT                 = 61; /** (gps_rtk_opt_t) RTK options - requires little endian CPU. */
	public static final int DID_REFERENCE_PIMU              = 62; /** (pimu_t) Reference or truth IMU used for manufacturing calibration and testing */
	public static final int DID_MANUFACTURING_INFO          = 63; /** INTERNAL USE ONLY (manufacturing_info_t) Manufacturing info */
	public static final int DID_BIT                         = 64; /** (bit_t) System built-in self-test */
	public static final int DID_INS_3                       = 65; /** (ins_3_t) Inertial navigation data with quaternion NED to body rotation and ECEF position. */
	public static final int DID_INS_4                       = 66; /** (ins_4_t) INS output: quaternion rotation w/ respect to ECEF, ECEF position. */
	public static final int DID_INL2_NED_SIGMA              = 67; /** (inl2_ned_sigma_t) Standard deviation of INL2 EKF estimates in the NED frame. */
	public static final int DID_STROBE_IN_TIME              = 68; /** (strobe_in_time_t) Timestamp for input strobe. */
	public static final int DID_GPS1_RAW                    = 69; /** (gps_raw_t) GPS raw data for rover (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
	public static final int DID_GPS2_RAW                    = 70; /** (gps_raw_t) GPS raw data for rover (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
	public static final int DID_WHEEL_ENCODER               = 71; /** (wheel_encoder_t) Wheel encoder data to be fused with GPS-INS measurements, set DID_GROUND_VEHICLE for configuration before sending this message */
	public static final int DID_DIAGNOSTIC_MESSAGE          = 72; /** (diag_msg_t) Diagnostic message */
	public static final int DID_SURVEY_IN                   = 73; /** (survey_in_t) Survey in, used to determine position for RTK base station. Base correction output cannot run during a survey and will be automatically disabled if a survey is started. */
	public static final int DID_CAL_SC_INFO                 = 74; /** INTERNAL USE ONLY (sensor_cal_info_t) */
	public static final int DID_PORT_MONITOR                = 75; /** (port_monitor_t) Data rate and status monitoring for each communications port. */
	public static final int DID_RTK_STATE                   = 76; /** INTERNAL USE ONLY (rtk_state_t) */
	public static final int DID_RTK_PHASE_RESIDUAL          = 77; /** INTERNAL USE ONLY (rtk_residual_t) */
	public static final int DID_RTK_CODE_RESIDUAL           = 78; /** INTERNAL USE ONLY (rtk_residual_t) */
	public static final int DID_RTK_DEBUG                   = 79; /** INTERNAL USE ONLY (rtk_debug_t) */
	public static final int DID_EVB_STATUS                  = 80; /** (evb_status_t) EVB monitor and log control interface. */
	public static final int DID_EVB_FLASH_CFG               = 81; /** (evb_flash_cfg_t) EVB configuration. */
	public static final int DID_EVB_DEBUG_ARRAY             = 82; /** INTERNAL USE ONLY (debug_array_t) */
	public static final int DID_EVB_RTOS_INFO               = 83; /** (evb_rtos_info_t) EVB-2 RTOS information. */
	// public static final int DID_UNUSED_84                = 84; /** Unused */
	public static final int DID_IMU_MAG                     = 85; /** (imu_mag_t) DID_IMU + DID_MAGNETOMETER. Only one of DID_IMU_MAG or DID_PIMU_MAG should be streamed simultaneously. */
	public static final int DID_PIMU_MAG                    = 86; /** (pimu_mag_t) DID_PIMU + DID_MAGNETOMETER. Only one of DID_IMU_MAG or DID_PIMU_MAG should be streamed simultaneously. */
	public static final int DID_GROUND_VEHICLE				= 87; /** (ground_vehicle_t) Static configuration for wheel transform measurements. */
	public static final int DID_POSITION_MEASUREMENT		= 88; /** (pos_measurement_t) External position estimate */
	public static final int DID_RTK_DEBUG_2                 = 89; /** INTERNAL USE ONLY (rtk_debug_2_t) */
	public static final int DID_CAN_CONFIG					= 90; /** (can_config_t) Addresses for CAN messages*/
	public static final int DID_GPS2_RTK_CMP_REL            = 91; /** (gps_rtk_rel_t) Dual GNSS RTK compassing / moving base to rover (GPS 1 to GPS 2) relative info. */
	public static final int DID_GPS2_RTK_CMP_MISC           = 92; /** (gps_rtk_misc_t) RTK Dual GNSS RTK compassing related data. */
	public static final int DID_EVB_DEV_INFO                = 93; /** (dev_info_t) EVB device information */
	public static final int DID_INFIELD_CAL                 = 94; /** (infield_cal_t) Measure and correct IMU calibration error.  Estimate INS rotation to align INS with vehicle. */
	public static final int DID_REFERENCE_IMU               = 95; /** (imu_t) Raw reference or truth IMU used for manufacturing calibration and testing. Input from testbed. */
	public static final int DID_IMU3_RAW                    = 96; /** (imu3_t) Triple IMU data calibrated from DID_IMU3_UNCAL.  We recommend use of DID_IMU or DID_PIMU as they are oversampled and contain less noise. */
	public static final int DID_IMU_RAW                     = 97; /** (imu_t) IMU data averaged from DID_IMU3_RAW.  Use this IMU data for output data rates faster than DID_FLASH_CONFIG.startupNavDtMs.  Otherwise we recommend use of DID_IMU or DID_PIMU as they are oversampled and contain less noise. */

	// Adding a new data id?
	// 1] Add it above and increment the previous number, include the matching data structure type in the comments
	// 2] Add flip doubles and flip strings entries in data_sets.c
	// 3] Add data id to ISDataMappings.cpp
	// 4] Increment DID_COUNT
	// 5) Update the DIDs in IS-src/python/src/ci_hdw/data_sets.py
	// 6] Test!

	/** Count of data ids (including null data id 0) - MUST BE MULTPLE OF 4 and larger than last DID number! */
	public static final int DID_COUNT		= 120;	// Used in SDK
	public static final int DID_COUNT_UINS	= 100;	// Used in uINS
	public static double EARTH_RADIUS =	6356752.0; // meters

	/** Maximum number of data ids */
	public static final int DID_MAX_COUNT  = 256;

	public static byte[] test_imx() {
		Did_ins_1 did_ins_1 = new Did_ins_1();
		did_ins_1.week = 12345;
		did_ins_1.timeOfWeek = 1234.5678;
		did_ins_1.insStatus =0x12345678;
		did_ins_1.hdwStatus =0x87654321;
		did_ins_1.theta[0] = (float) 0.12345;
		did_ins_1.theta[1] = (float) 0.54321;
		did_ins_1.theta[2] = (float) 0.01234;
		did_ins_1.uvw[0] = (float) 12.345;
		did_ins_1.uvw[1] = (float) 54.321;
		did_ins_1.uvw[2] = (float) 1.234;
		did_ins_1.lla[0] = 40.197590019999815;
		did_ins_1.lla[1] = -111.62147172;
		did_ins_1.lla[2] = 1408.565263999626;
		did_ins_1.ned[0] = (float) 1.0;
		did_ins_1.ned[1] = (float) 2.0;
		did_ins_1.ned[2] = (float) 3.0;
		System.out.println("did_ins_1 = "+did_ins_1);
		ByteBuffer bb = ByteBuffer.allocate(1000);
		bb.order(ByteOrder.LITTLE_ENDIAN);
		int result_bytes = did_ins_1.pack(bb);
		System.out.println ("Packed size = "+result_bytes);
		bb.rewind();
		Did_ins_1 did_ins_1_2 = new Did_ins_1(bb);
		System.out.println("did_ins_1_2 = "+did_ins_1_2);
		return bb.array();
	}
//	static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
//	RotationConvention.VECTOR_OPERATOR
//RotationOrder.YXZ, ROT_CONV
	// move to did?
	public static double [] quatEnu (double [] quat_ned) {
		Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5),	Math.sqrt(0.5), 0, true);
		Rotation quat_rot = new Rotation(quat_ned[0],quat_ned[1],quat_ned[2],quat_ned[3],true);
		Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
		return new double[] {
				quat_enu_rot.getQ0(),
				quat_enu_rot.getQ1(),
				quat_enu_rot.getQ2(),
				quat_enu_rot.getQ3()};
	}
	
	public static double [] applyQuaternionTo(double[]quat, double[] vector, boolean inverse) {
		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
		double [] rslt = new double[3];
		if (inverse) {
			ims_rot.applyInverseTo(vector, rslt);

		} else {
			ims_rot.applyTo(vector, rslt);
		}
		return rslt;
	}

	public static double [] applyQuaternionToQuaternion(
			double[] quat,      // additional correction
			double[] targ_quat, // quaternion to be corrected
			boolean inverse) {
		if ((quat==null) || (targ_quat == null)) {
			System.out.println("applyQuaternionToQuaternion(): returning null");
			return null;
		}
		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
		Rotation targ_rot = new Rotation(targ_quat[0],targ_quat[1],targ_quat[2],targ_quat[3],true);
		Rotation rslt_rot;
		if (inverse) {
			rslt_rot=ims_rot.applyInverseTo(targ_rot);

		} else {
			rslt_rot=ims_rot.applyTo(targ_rot);
		}
		return new double[] {
				rslt_rot.getQ0(),
				rslt_rot.getQ1(),
				rslt_rot.getQ2(),
				rslt_rot.getQ3()};
	}

	/**
	 * Get up (for IMS) vector relative to the camera reference frame
	 * @param ims_atr mount to camera correction
	 * @return {x,y,z} of the IMS-up relative to the camera frame (reference scene)
	 */
	public static double [] getUpAxis(
			double [] ims_atr ) {// -> mount_to_cam
		Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
				ims_atr[0],    ims_atr[1],    ims_atr[2]);
		return mount_to_cam.applyInverseTo(new Vector3D(0, 0, 1)).toArray();
	}
	
	
	public static double [] quaternionImsToCam(
			double [] quat,       // ims_to_ned // normally d2_ref.getQEnu(), // current IMS absolute orientation relative to ENU world frame
			double [] ims_atr,    // -> IMS_to_cam (angles) 
			double [] quat_ort) { // -> ims_to_mount_ortho additional 90-degree rotaions
		Rotation ims_to_mount_ortho = new Rotation(quat_ort[0],quat_ort[1],quat_ort[2],quat_ort[3],true);
		Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
		Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
				ims_atr[0],    ims_atr[1],    ims_atr[2]);
		Rotation mount_to_ned = ims_to_mount_ortho.applyTo(ims_to_ned);
		Rotation cam_quat = mount_to_cam.applyTo(mount_to_ned);
		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
	}

	// from ims frame to camera frame
	public static double [] quaternionImsToCam(
			double [] ims_atr, // -> mount_to_cam
			double [] quat_ort) { // -> ims_to_mount_ortho
		Rotation ims_to_mount_ortho = new Rotation(quat_ort[0],quat_ort[1],quat_ort[2],quat_ort[3],true);
		Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
				ims_atr[0],    ims_atr[1],    ims_atr[2]);
		Rotation cam_quat = mount_to_cam.applyTo(ims_to_mount_ortho);
		return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
	}
	
	/**
	 * Adjsut IMS orientation relative to the camera (or opposite?) by a correction quaternion
	 * @param ims_atr original ATR of the IMS mount
	 * @param corr_q correction quaternion
	 * @return corrected ATR of the IMU mount
	 */
	public static double [] adjustMountAtrByQuat(
			double [] ims_atr,
			double [] corr_q) {
		Rotation rot_ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
				ims_atr[0],    ims_atr[1],    ims_atr[2]);
		Rotation rot_corr = new Rotation(corr_q[0],corr_q[1],corr_q[2],corr_q[3],false);
		Rotation rot = rot_ims_atr.applyTo(rot_corr);
		return rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);		
	}
	
	public static double [] quatToCamAtr(double[]quat) {
		Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
		return rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);		
	}
	
	public static double [] nedFromLla (double [] lla, double [] lla_ref) {
		double [] ned = new double[] { 
				EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,
				EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)*(lla[1] - lla_ref[1]) * Math.PI / 180.0,
				-(lla[2] - lla_ref[2])
		};
		return ned;
	}
	
	public static double [] enuFromLla (double [] lla, double [] lla_ref) {
		double [] ned = new double[] { 
				EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)*(lla[1] - lla_ref[1]) * Math.PI / 180.0,
				EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,
				(lla[2] - lla_ref[2])
		};
		return ned;
	}

	/*
	 * Offset Latitude, Longitude, Altitude by North, East, Up (in meters) 
	 */
	public static double [] offsetLlaNED(double [] lla, double [] ned) {
		double [] offset_lla = new double [] {
				lla[0] + (ned[0]/EARTH_RADIUS / Math.PI * 180 / Math.cos(lla[0])), // degrees
				lla[1] + (ned[1]/EARTH_RADIUS / Math.PI * 180), // degrees
				lla[2] - ned[2]
		};
		return offset_lla;
	}
}
