/**
 **
 ** Did_ins_1.java - InertialSense DID_INS_1 message decoder for INS output data
 **
 ** Copyright (C) 2023-2025 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  Did_ins_1.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.nio.BufferUnderflowException;
//import java.io.Serializable;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Properties;

import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;

//public class Did_ins_1 implements Serializable {
//	static final long serialVersionUID = 4;
public class Did_ins_1 extends Did_ins <Did_ins_1>{
	/** GPS number of weeks since January 6th, 1980 */
//	public int				week;      // uint32_t
	/** GPS time of week (since Sunday morning) in seconds */
//	public double			timeOfWeek;
	/** INS status flags (eInsStatusFlags). Copy of DID_SYS_PARAMS.insStatus */
//	public int				insStatus; //uint32_t
	/** Hardware status flags (eHdwStatusFlags). Copy of DID_SYS_PARAMS.hdwStatus */
//	public int				hdwStatus; // uint32_t
	/** Euler angles: roll, pitch, yaw in radians with respect to NED */
	public float []		theta = new float[3];
	/** Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )". */
//	public float []		uvw =   new float [3];
	/** WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters) */
//	public double []		lla =   new double [3];
	/** North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude */
	public float []		ned = new float [3];

	public double [] getTheta() {
		return new double [] {theta[0], theta[1], theta[2]};
	}
//	public double [] getUvw() {
//		return new double [] {uvw[0], uvw[1], uvw[2]};
//	}
	public double [] getNed() {
		return new double [] {ned[0], ned[1], ned[2]};
	}
	
	public Did_ins_1 (ByteBuffer bb) {
		bb.order(ByteOrder.LITTLE_ENDIAN);
		try {
			week=      bb.getInt();
			timeOfWeek=bb.getDouble();
			insStatus= bb.getInt();
			hdwStatus= bb.getInt();
			theta[0]=  bb.getFloat();  theta[1] = bb.getFloat();  theta[2]= bb.getFloat();
			uvw[0]=    bb.getFloat();  uvw[1] =   bb.getFloat();  uvw[2]=   bb.getFloat();
			lla[0]=    bb.getDouble(); lla[1] =   bb.getDouble(); lla[2]=   bb.getDouble();
			ned[0]=    bb.getFloat();  ned[1] =   bb.getFloat();  ned[2]=   bb.getFloat();
		} catch (BufferUnderflowException e) {
			// TODO Auto-generated catch block
			good = false;
			return; // e.printStackTrace();
		}
	}
	public Did_ins_1() {}
	public Did_ins_1(String prefix, Properties properties) {
		getProperties(prefix, properties);
	}
	

	public Did_ins_1 interpolate(double frac, Did_ins_1 next_did) {
		Did_ins_1 new_did = new Did_ins_1();
		interpolateBase(frac, next_did, new_did);
    	for (int i = 0; i < lla.length; i++) {
    		new_did.theta[i] =   interpolateFloat(frac, theta[i], next_did.theta[i]) ;
//    		new_did.uvw[i] =     interpolateFloat(frac, uvw[i],   next_did.uvw[i]) ;
    		new_did.ned[i] =     interpolateFloat(frac, ned[i],   next_did.ned[i]) ;
    	}
		return new_did;
	}
	
	
	public int pack(ByteBuffer bb) {
		int p_start = bb.position();
		bb.putInt(week);
		bb.putDouble(timeOfWeek);
		bb.putInt(insStatus);
		bb.putInt(hdwStatus);
		bb.putFloat(theta[0]); bb.putFloat(theta[1]); bb.putFloat(theta[2]);
		bb.putFloat(uvw[0]);   bb.putFloat(uvw[1]);   bb.putFloat(uvw[2]);
		bb.putDouble(lla[0]);  bb.putDouble(lla[1]);  bb.putDouble(lla[2]);
		bb.putFloat(ned[0]);   bb.putFloat(ned[1]);   bb.putFloat(ned[2]);
		int p_end = bb.position();
		return p_end - p_start;
	}
	
	public String toString() {
		String s = "DID_INS_1\n";
		s += String.format("week:       %d\n", week); 
		s += String.format("timeOfWeek: %f\n", timeOfWeek); 
		s += String.format("insStatus:  0x%x\n", insStatus); 
		s += String.format("hdwStatus:  0x%x\n", hdwStatus); 
		s += String.format("theta:      %f, %f, %f\n", theta[0], theta[1], theta[2]); 
		s += String.format("uvw:        %f, %f, %f\n", uvw[0], uvw[1], uvw[2]); 
		s += String.format("lla:        %f, %f, %f\n", lla[0], lla[1], lla[2]); 
		s += String.format("ned:        %f, %f, %f",   ned[0], ned[1], ned[2]); 
		return s;
	}

	public void getProperties(String prefix, Properties properties) {
		super.getProperties(prefix, properties);
		if (properties.getProperty(prefix+"theta")!=null) 	   this.theta= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"theta"),3);
//		if (properties.getProperty(prefix+"uvw")!=null) 	   this.uvw=   IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"uvw"),3);
		if (properties.getProperty(prefix+"ned")!=null) 	   this.ned=   IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"ned"),3);
	}
	public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
		properties = super.setProperties(prefix, properties);
		properties.setProperty(prefix+"theta",        IntersceneMatchParameters.floatsToString(this.theta));		
//		properties.setProperty(prefix+"uvw",          IntersceneMatchParameters.floatsToString(this.uvw));		
		properties.setProperty(prefix+"ned",          IntersceneMatchParameters.floatsToString(this.ned));		
		return properties;
	}	

}