/**
 **
 ** Did_ins_2.java - InertialSense DID_INS_2 message decoder for INS output data
 **
 ** Copyright (C) 2023-2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  Did_ins_2.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 org.apache.commons.math3.geometry.euclidean.threed.Rotation;

import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;

//public class Did_ins_1 implements Serializable {
//	static final long serialVersionUID = 4;
public class Did_ins_2 extends Did_ins <Did_ins_2>{
	/** 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 */
	/** Quaternion */
	public float []		qn2b = new float[4]; // orientation relative to NED
	/** 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];

	public Did_ins_2 (ByteBuffer bb) {
		bb.order(ByteOrder.LITTLE_ENDIAN);	
		try {
			week=      bb.getInt();
			timeOfWeek=bb.getDouble();
			insStatus= bb.getInt();
			hdwStatus= bb.getInt();
			qn2b[0]=  bb.getFloat();  qn2b[1] = bb.getFloat();  qn2b[2]= bb.getFloat(); qn2b[3]= 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();
		} catch (BufferUnderflowException e) {
			// TODO Auto-generated catch block
			good = false;
			return; // e.printStackTrace();
		}
	}
	public Did_ins_2() {}
	public Did_ins_2(String prefix, Properties properties) {
		getProperties(prefix, properties);
	}
	
	// recovering missing Did_ins_2
	/**
	 * Reading Did_ins_1 from bb and adding pre-calculated quaternion
	 * @param bb buffer containing Did_ins_1
	 * @param quat pre-calculated quaternion (calculation needs the sequence so it can not be obtained from just angles)
	 */
	public Did_ins_2 (ByteBuffer bb,
			float [] quat) {
		Did_ins_1 did_ins1 = new  Did_ins_1(bb);
		// copy common fields
		good       = did_ins1.good;
		week       = did_ins1.week;
		timeOfWeek = did_ins1.timeOfWeek;
		insStatus  = did_ins1.insStatus;
		hdwStatus  = did_ins1.hdwStatus;
		uvw        = did_ins1.uvw;
		lla        = did_ins1.lla;
		// add specific field - quaternion
		qn2b =       quat;
	}	
	
	public double [] getQn2b() {
		return new double [] {qn2b[0], qn2b[1], qn2b[2], qn2b[3]};
	}
	
	public double [] getQEnu () {// apply absolute orientation to ortho rotation
		Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5),	Math.sqrt(0.5), 0, true);
		Rotation quat_rot = new Rotation(qn2b[0],qn2b[1],qn2b[2],qn2b[3],true);
		Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned); // apply absolute orientation to ortho rotation
		return new double[] {
				quat_enu_rot.getQ0(),
				quat_enu_rot.getQ1(),
				quat_enu_rot.getQ2(),
				quat_enu_rot.getQ3()};
	}

	
//	public double [] getUvw() {
//		return new double [] {uvw[0], uvw[1], uvw[2]};
//	}

	public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) {
		Did_ins_2 new_did = new Did_ins_2();
		interpolateBase(frac, next_did, new_did);
    	for (int i = 0; i < qn2b.length; i++) {
    		new_did.qn2b[i] =   interpolateFloat(frac, qn2b[i], next_did.qn2b[i]) ;
    	}    	
//    	for (int i = 0; i < uvw.length; i++) {
//    		new_did.uvw[i] =   interpolateFloat(frac, uvw[i], next_did.uvw[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(qn2b[0]); bb.putFloat(qn2b[1]); bb.putFloat(qn2b[2]); bb.putFloat(qn2b[3]);
		bb.putFloat(uvw[0]);   bb.putFloat(uvw[1]);   bb.putFloat(uvw[2]);
		bb.putDouble(lla[0]);  bb.putDouble(lla[1]);  bb.putDouble(lla[2]);
		int p_end = bb.position();
		return p_end - p_start;
	}
	
	public String toString() {
		String s = "DID_INS_2\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("qn2b:       %f, %f, %f, %f\n", qn2b[0], qn2b[1], qn2b[2], qn2b[3]); 
		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]); 
		return s;
	}

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