/**
 **
 ** Did_ins.java - InertialSense DID_INS message decoder for navigation solution data
 **
 ** Copyright (C) 2023-2025 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  Did_ins.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.time.LocalDate;
import java.time.LocalDateTime;
import java.util.Properties;

import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;

public abstract class Did_ins <T extends Did_ins <T>>{
	final static LocalDateTime DT_01_06_1980 = LocalDateTime.of(1980,1,6,0,0);
	final static int WEEK_SECONDS = 7 * 24 * 3600;
	
	public boolean good = true;
	/** 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
	/** 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];

	/** for DID_INS verification */
	public static int eHdwStatusFlags_mask = 0xffffffd0;
    public static int eHdwStatusFlags_data = 0x0008007f;
//	public static int eInsStatusFlags_mask = 0x8c0059ee;
//	public static int eInsStatusFlags_mask = 0x8c00596e; // once in 300-400 - GPS aided heading
    //was before 11/2025
//	public static int eInsStatusFlags_mask = 0x8800596e; // once in 300-400 - INS_STATUS_RTK_COMPASSING_VALID
    
	// after 11/2025
    // isDidSane(): bad insStatus=0x156351b7 (0x156351b7 ^ 0x00635177) & 0x8800596e = 0x00000040
    
	public static int eInsStatusFlags_mask = 0x8800592e; // once in 300-400 - INS_STATUS_RTK_COMPASSING_VALID
	
	//
    public static int eInsStatusFlags_data = 0x00635177;
	public static int MIN_WEEK =             1766; // -10 yrs
	public static int MAX_WEEK =             2806; // +10 yrs
	
	/**
	 * Convert Euler angles to quaternion, ambiguous sign
	 * @param roll  roll angle in radians
	 * @param pitch pitch angle in radians
	 * @param yaw   yaw angle in radians
	 * @return quaternion as a 4-element double array (w,x,y,z)
	 */
	public static double [] eulerToQuat(
			double roll,
			double pitch,
			double yaw) { // roll (x), pitch (y), yaw (z), angles are in radians
	    double cr = Math.cos(roll * 0.5);
	    double sr = Math.sin(roll * 0.5);
	    double cp = Math.cos(pitch * 0.5);
	    double sp = Math.sin(pitch * 0.5);
	    double cy = Math.cos(yaw * 0.5);
	    double sy = Math.sin(yaw * 0.5);
	    double [] q = new double [4];
	    q[0] = cr * cp * cy + sr * sp * sy;
	    q[1] = sr * cp * cy - cr * sp * sy;
	    q[2] = cr * sp * cy + sr * cp * sy;
	    q[3] = cr * cp * sy - sr * sp * cy;
	    return q;
	}

	public static float [] eulerToQuat(
			float roll,
			float pitch,
			float yaw) { // roll (x), pitch (y), yaw (z), angles are in radians
	    double cr = Math.cos(roll * 0.5);
	    double sr = Math.sin(roll * 0.5);
	    double cp = Math.cos(pitch * 0.5);
	    double sp = Math.sin(pitch * 0.5);
	    double cy = Math.cos(yaw * 0.5);
	    double sy = Math.sin(yaw * 0.5);
	    float [] q = new float [4];
	    q[0] = (float) (cr * cp * cy + sr * sp * sy);
	    q[1] = (float) (sr * cp * cy - cr * sp * sy);
	    q[2] = (float) (cr * sp * cy + sr * cp * sy);
	    q[3] = (float) (cr * cp * sy - sr * sp * cy);
	    return q;
	}
	
	
	/**
	 * Convert Euler angles to quaternion, ambiguous sign
	 * @param rpy combined angles array {roll, pitch, yaw}
	 * @return quaternion as a 4-element double array (w,x,y,z)
	 */
	public static double [] eulerToQuat(
			double [] rpy) {
		return eulerToQuat(rpy, null);
	}
	
	public static float [] eulerToQuat(
			float  [] rpy) {
		return eulerToQuat(rpy, null);
	}	

	/**
	 * Convert Euler angles to quaternion, positive q[0] (w)
	 * @param rpy combined angles array {roll, pitch, yaw}
	 * @return quaternion as a 4-element double array (w,x,y,z), w >=0
	 */
	public static float [] eulerToQuatPosQ0(
			float [] rpy) {
		float [] q = eulerToQuat(rpy, null);
	    if (q[0]< 0) {
	    	for (int i = 0; i < q.length; i++) {
	    		q[i] = -q[i];
	    	}
	    }
	    return q;
	}	

	public static double [] eulerToQuatPosQ0(
			double [] rpy) {
		double [] q = eulerToQuat(rpy, null);
	    if (q[0]< 0) {
	    	for (int i = 0; i < q.length; i++) {
	    		q[i] = -q[i];
	    	}
	    }
	    return q;
	}	
	
	/**
	 * Convert Euler angles to quaternion, sign is selected to minimize 4-distance from the previous quaternion, valiud for interpolation
	 * @param rpy    combined angles array {roll, pitch, yaw}
	 * @param prev_q previous (earlier in time) quaternion value
	 * @return quaternion as a 4-element double array (w,x,y,z), sign is selected to minimize (q0-prevq0)^2 + (q1-prevq1)^2 + (q2-prevq1)^2 + (q1-prevq1)^2
	 */
	public static double [] eulerToQuat(
			double [] rpy,
			double [] prev_q) {
		double [] q = eulerToQuat(
				rpy[0], //double roll,
				rpy[1], // double pitch,
				rpy[2]);
		if (prev_q != null) {
			double inv2=0; // >0 - invert quaternion to match the previous one
			for (int i = 0; i< q.length; i++) {
				double ds = q[i] - prev_q[i];
				double di = q[i] + prev_q[i];
				inv2+=ds*ds - di*di;
			}
			if (inv2 > 0) {
		    	for (int i = 0; i < q.length; i++) {
		    		q[i] = -q[i];
		    	}
			}
		}
	    return q;
	}

	public static float [] eulerToQuat(
			float [] rpy,
			float [] prev_q) {
		float [] q = eulerToQuat(
				rpy[0], //double roll,
				rpy[1], // double pitch,
				rpy[2]);
		if (prev_q != null) {
			float inv2=0; // >0 - invert quaternion to match the previous one
			for (int i = 0; i< q.length; i++) {
				float ds = q[i] - prev_q[i];
				float di = q[i] + prev_q[i];
				inv2+=ds*ds - di*di;
			}
			if (inv2 > 0) {
		    	for (int i = 0; i < q.length; i++) {
		    		q[i] = -q[i];
		    	}
			}
		}
	    return q;
	}
	
	
	
	
	static int interpolateInt(double frac, int v_this, int v_next) {
		return (int) Math.round(frac * v_next + (1.0 - frac) * v_this);
	}
	
	static float interpolateFloat(double frac, float v_this, float v_next) {
		return (float) (frac * v_next + (1.0 - frac) * v_this);
	}

	static double interpolateDouble(double frac, double v_this, double v_next) {
		return frac * v_next + (1.0 - frac) * v_this;
	}
	
	public double getDoubleTime() {
		return ((double) week) * WEEK_SECONDS + timeOfWeek;
	}
	
	public int [] getWeekTimeMs() { // for compatibility with DID_STROBE_IN_TIME
		return new int [] { week, (int)Math.round(timeOfWeek * 1000.0)}; 
	}
	
	public static LocalDateTime getLocalDateTime(int week, double tow) {
		/** GPS number of weeks since January 6th, 1980 */
		/** GPS time of week (since Sunday morning) in seconds */
		int seconds = (int) Math.floor(tow);
		int nanos = (int) Math.floor((tow-seconds)*1E9);
		return DT_01_06_1980.plusWeeks(week).
				plusSeconds(seconds).plusNanos(nanos);
	}
	
	public LocalDateTime getLocalDateTime() {
		return getLocalDateTime(week, timeOfWeek);
	}
	
	
	public boolean isDidSane() {
		if (!good) { // BufferUnderflowException when creating
			return false;
		}
		if ((week < MIN_WEEK) || (week > MAX_WEEK)) {
			System.out.println("isDidSane(): bad week = "+week+
					" - should be in range ["+MIN_WEEK+","+MAX_WEEK+"]");
			return false;
		}
		if ((timeOfWeek < 0) || (timeOfWeek > WEEK_SECONDS)) {
			System.out.println("isDidSane(): bad timeOfWeek = "+timeOfWeek+
					" - should be in range [0,"+WEEK_SECONDS+"]");
			return false;
		}
		if (((hdwStatus ^ eHdwStatusFlags_data) & eHdwStatusFlags_mask) != 0) {
			System.out.println(String.format("isDidSane(): bad hdwStatus=0x%08x"+
					" (0x%08x ^ 0x%08x) & 0x%08x = 0x%08x",
					hdwStatus, hdwStatus, eHdwStatusFlags_data,eHdwStatusFlags_mask,
					(hdwStatus ^ eHdwStatusFlags_data) & eHdwStatusFlags_mask));
			return false;
		}
		if (((insStatus ^ eInsStatusFlags_data) & eInsStatusFlags_mask) != 0) {
			System.out.println(String.format("isDidSane(): bad insStatus=0x%08x"+
					" (0x%08x ^ 0x%08x) & 0x%08x = 0x%08x",
					insStatus, insStatus, eInsStatusFlags_data,eInsStatusFlags_mask,
					(insStatus ^ eInsStatusFlags_data) & eInsStatusFlags_mask));
			return false;
		}
		if (    (lla[0] <-90)  || (lla[0] > 90) || // latitude
				(lla[1] <-180) || (lla[1] > 180) || // longitude
				(lla[2] <-10)  || (lla[1] > 20000)) { // altitude
			System.out.println("isDidSane(): bad lla=["+
				lla[0]+", "+lla[1]+", "+lla[2]+"], timeOfWeek="+timeOfWeek);
			return false;
		}
		return true;
	}
	
//https://www.swtestacademy.com/return-subclass-instance-java-generics/	
	public void interpolateBase(double frac, T next_did, T new_did) {
		new_did.week =     this.week;
    	double tow_next =  next_did.timeOfWeek;
    	if (next_did.week > this.week) {
    		tow_next +=  (next_did.week - this.week) *7 * 24 * 3600; // week in seconds
    	}
    	new_did.timeOfWeek = interpolateDouble ( frac, this.timeOfWeek, tow_next);
    	while (new_did.timeOfWeek >= WEEK_SECONDS) {
    		new_did.timeOfWeek -= WEEK_SECONDS;
    		new_did.week ++;
    	}
    	new_did.insStatus =     this.insStatus;
    	new_did.hdwStatus =     this.hdwStatus;
    	for (int i = 0; i < lla.length; i++) {
    		new_did.lla[i] =   interpolateDouble(frac, lla[i], next_did.lla[i]) ;
    	}
    	for (int i = 0; i < uvw.length; i++) {
    		new_did.uvw[i] =   interpolateFloat(frac, uvw[i], next_did.uvw[i]) ;
    	}    	
	}
	
	public void getProperties(String prefix, Properties properties) {
		if (properties.getProperty(prefix+"week")!=null)       this.week= Integer.parseInt(properties.getProperty(prefix+"week"));
		if (properties.getProperty(prefix+"timeOfWeek")!=null) this.timeOfWeek= Double.parseDouble(properties.getProperty(prefix+"timeOfWeek"));
		if (properties.getProperty(prefix+"insStatus")!=null)  this.insStatus= Integer.parseInt(properties.getProperty(prefix+"insStatus"));
		if (properties.getProperty(prefix+"hdwStatus")!=null)  this.hdwStatus= Integer.parseInt(properties.getProperty(prefix+"hdwStatus"));
		if (properties.getProperty(prefix+"lla")!=null) 	   this.lla= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"lla"),3);
		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
		if (properties == null) {
			properties = new Properties();
		}
		properties.setProperty(prefix+"week",       this.week+"");
		properties.setProperty(prefix+"timeOfWeek", this.timeOfWeek+"");
		properties.setProperty(prefix+"insStatus",  this.insStatus+"");
		properties.setProperty(prefix+"hdwStatus",  this.hdwStatus+"");
		properties.setProperty(prefix+"lla",        IntersceneMatchParameters.doublesToString(this.lla));
		properties.setProperty(prefix+"uvw",          IntersceneMatchParameters.floatsToString(this.uvw));		
		return properties;
	}
	public double [] getUvw() {
		return new double [] {uvw[0], uvw[1], uvw[2]};
	}
	
}