/**
 **
 ** Did_gps_pos.java - InertialSense DID_GPS_POS message decoder for GPS position data
 **
 ** Copyright (C) 2023-2025 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  Did_gps_pos.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.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Properties;

import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;

/** (DID_GPS1_POS, DID_GPS1_UBX_POS, DID_GPS2_POS) GPS position data */
public class Did_gps_pos {
	
	/* (DID_GPS1_POS, DID_GPS1_UBX_POS, DID_GPS2_POS) GPS position data
	typedef struct PACKED
	{
		// GPS number of weeks since January 6th, 1980
		uint32_t                week;
		// GPS time of week (since Sunday morning) in milliseconds
		uint32_t                timeOfWeekMs;
		// (see eGpsStatus) GPS status: [0x000000xx] number of satellites used, [0x0000xx00] fix type, [0x00xx0000] status flags, NMEA input flag
		uint32_t                status;
		// Position in ECEF {x,y,z} (m)
		double					ecef[3];
		// Position - WGS84 latitude, longitude, height above ellipsoid (not MSL) (degrees, m)
		double					lla[3];
		// Height above mean sea level (MSL) in meters
		float					hMSL;
		// Horizontal accuracy in meters
		float					hAcc;
		// Vertical accuracy in meters
		float					vAcc;
		// Position dilution of precision (unitless)
		float                   pDop;
		// Average of all non-zero satellite carrier to noise ratios (signal strengths) in dBHz
		float                   cnoMean;
		// Time sync offset between local time since boot up to GPS time of week in seconds.  Add this to IMU and sensor time to get GPS time of week in seconds.
		double                  towOffset;
		// GPS leap second (GPS-UTC) offset. Receiver's best knowledge of the leap seconds offset from UTC to GPS time. Subtract from GPS time of week to get UTC time of week. (18 seconds as of December 31, 2016)
		uint8_t					leapS;
		// Number of satellites used
		uint8_t					satsUsed;
		// Standard deviation of cnoMean over past 5 seconds (dBHz x10)
		uint8_t					cnoMeanSigma;
		// Reserved for future use
		uint8_t					reserved;
	} gps_pos_t;
	*/
	
//	public final static int WEEK_MS = 7 * 24 * 3600 * 1000; // milliseconds in a week// Did_pimu.
	/** GPS number of weeks since January 6th, 1980 */
	public int 	     week;
	/** GPS time of week (since Sunday morning) in milliseconds */
	public int 	     timeOfWeekMs;
	/** (see eGpsStatus) GPS status: [0x000000xx] number of satellites used, [0x0000xx00] fix type, [0x00xx0000] status flags, NMEA input flag */
	public int 	     status;
	/** Position in ECEF {x,y,z} (m) */
	public double [] ecef = new double[3];
	/** Position - WGS84 latitude, longitude, height above ellipsoid (not MSL) (degrees, m) */
	public double [] lla =  new double[3];
	/** Height above mean sea level (MSL) in meters */
	public float	 hMSL;
	/** Horizontal accuracy in meters */
	public float	 hAcc;
	/** Vertical accuracy in meters */
	public float	 vAcc;
	/** Position dilution of precision (unitless) */
	public float     pDop;
	/** Average of all non-zero satellite carrier to noise ratios (signal strengths) in dBHz */
	public float     cnoMean;
	/** Time sync offset between local time since boot up to GPS time of week in seconds.  Add this to IMU and sensor time to get GPS time of week in seconds. */
	public double    towOffset;
	/** GPS leap second (GPS-UTC) offset. Receiver's best knowledge of the leap seconds offset from UTC to GPS time. Subtract from GPS time of week to get UTC time of week. (18 seconds as of December 31, 2016) */
	public byte		 leapS;
	/** Number of satellites used */
	public byte		 satsUsed;
	/** Standard deviation of cnoMean over past 5 seconds (dBHz x10) */
	public byte		 cnoMeanSigma;
	/** Reserved for future use */
	public byte		 reserved;
	
	public Did_gps_pos (ByteBuffer bb) { // getChar()
		bb.order(ByteOrder.LITTLE_ENDIAN);
		week =         bb.getInt();
		timeOfWeekMs = bb.getInt();
		status =       bb.getInt();
		ecef[0] =      bb.getDouble(); ecef[1] = bb.getDouble(); ecef[2] = bb.getDouble();
		lla[0] =       bb.getDouble(); lla[1] =  bb.getDouble(); lla[2] =  bb.getDouble();
		hMSL =         bb.getFloat();
		hAcc =         bb.getFloat();
		vAcc =         bb.getFloat();
		pDop =         bb.getFloat();
		cnoMean =      bb.getFloat();
		towOffset =    bb.getDouble();
		leapS =        bb.get();
		satsUsed =     bb.get();
		cnoMeanSigma = bb.get();
		reserved =     bb.get();
	}
	public Did_gps_pos() {}

	public Did_gps_pos(String prefix, Properties properties) {
		getProperties(prefix, properties);
	}

	public boolean isDidSane() { // add more!
		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]+"], timeOfWeekMs="+timeOfWeekMs);
			return false;
		}
		return true;
	}
	
	
	public Did_gps_pos interpolate(double frac, Did_gps_pos next_did) {
		Did_gps_pos new_did = new Did_gps_pos();
		double time_ms_this = timeOfWeekMs + Did_strobe_in_time.WEEK_MS * week;
		double time_ms_next = next_did.timeOfWeekMs + Did_strobe_in_time.WEEK_MS * next_did.week;
		long   time_ms =      (long) Did_ins.interpolateDouble(frac, time_ms_this, time_ms_next);
		new_did.week =         (int) Math.floor(time_ms / Did_strobe_in_time.WEEK_MS);
		new_did.timeOfWeekMs = (int) (time_ms - new_did.week * Did_strobe_in_time.WEEK_MS); 
		new_did.status =       status;
    	for (int i = 0; i < lla.length; i++) {
    		new_did.ecef[i] =  Did_ins.interpolateDouble(frac, ecef[i], next_did.ecef[i]) ;
    		new_did.lla[i] =   Did_ins.interpolateDouble(frac, lla[i],  next_did.lla[i]) ;
    	}
    	new_did.hMSL =      Did_ins.interpolateFloat (frac, hMSL,      next_did.hMSL);
    	new_did.hAcc =      Did_ins.interpolateFloat (frac, hAcc,      next_did.hAcc);
    	new_did.vAcc =      Did_ins.interpolateFloat (frac, vAcc,      next_did.vAcc);
    	new_did.pDop =      Did_ins.interpolateFloat (frac, pDop,      next_did.pDop);
    	new_did.cnoMean =   Did_ins.interpolateFloat (frac, cnoMean,   next_did.cnoMean);
    	new_did.towOffset = Did_ins.interpolateDouble(frac, towOffset, next_did.towOffset);
		new_did.leapS =        leapS;
		new_did.satsUsed =     satsUsed;
		new_did.cnoMeanSigma = cnoMeanSigma;
		new_did.reserved =     reserved;
		return new_did;
	}
	
	
	public int pack(ByteBuffer bb) {
		int p_start = bb.position();
		bb.putInt(week);
		bb.putInt(timeOfWeekMs);
		bb.putInt(status);
		bb.putDouble(ecef[0]); bb.putDouble(ecef[1]); bb.putDouble(ecef[2]); 
		bb.putDouble(lla[0]);  bb.putDouble(lla[1]);  bb.putDouble(lla[2]);
		bb.putFloat (hMSL);
		bb.putFloat (hAcc);
		bb.putFloat (vAcc);
		bb.putFloat (pDop);
		bb.putFloat (cnoMean);
		bb.putDouble(towOffset);
		bb.put      (leapS);
		bb.put      (satsUsed);
		bb.put      (cnoMeanSigma);
		bb.put      (reserved);
		int p_end = bb.position();
		return p_end - p_start;
	}
	
	public String toString() {
		String s = "DID_GPS_POS (DID_GPS1_POS, DID_GPS1_UBX_POS, DID_GPS2_POS)\n";
		s += String.format("week:         %d\n", week); 
		s += String.format("timeOfWeekMs: %d\n", timeOfWeekMs); 
		s += String.format("status:       0x%x\n", status); 
		s += String.format("ecef:         %f, %f, %f\n", ecef[0], ecef[1], ecef[2]); 
		s += String.format("lla:          %f, %f, %f\n", lla[0],  lla[1],  lla[2]); 
		s += String.format("hMSL:         %f\n",  hMSL); 
		s += String.format("hAcc:         %f\n",  hAcc); 
		s += String.format("vAcc:         %f\n",  vAcc); 
		s += String.format("pDop:         %f\n",  pDop); 
		s += String.format("cnoMean:      %f\n",  cnoMean); 
		s += String.format("towOffset:    %f\n",  towOffset); 
		s += String.format("leapS:        %d\n",  ((int) leapS) & 0xff); 
		s += String.format("satsUsed:     %d\n",  ((int) satsUsed) & 0xff); 
		s += String.format("cnoMeanSigma: %d\n",  ((int) cnoMeanSigma) & 0xff); 
		s += String.format("reserved:     %d",    ((int) reserved) & 0xff); 
		return s;
	}
	
 	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+"timeOfWeekMs")!=null) this.timeOfWeekMs= Integer.parseInt(properties.getProperty(prefix+"week"));
		if (properties.getProperty(prefix+"status")!=null)       this.status= Integer.parseInt(properties.getProperty(prefix+"status"));
		if (properties.getProperty(prefix+"ecef")!=null) 	     this.ecef= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ecef"),3);
		if (properties.getProperty(prefix+"lla")!=null) 	     this.lla= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"lla"),3);
		if (properties.getProperty(prefix+"hMSL")!=null)         this.hMSL= Float.parseFloat(properties.getProperty(prefix+"hMSL"));
		if (properties.getProperty(prefix+"hAcc")!=null)         this.hAcc= Float.parseFloat(properties.getProperty(prefix+"hAcc"));
		if (properties.getProperty(prefix+"vAcc")!=null)         this.vAcc= Float.parseFloat(properties.getProperty(prefix+"vAcc"));
		if (properties.getProperty(prefix+"pDop")!=null)         this.pDop= Float.parseFloat(properties.getProperty(prefix+"pDop"));
		if (properties.getProperty(prefix+"cnoMean")!=null)      this.cnoMean= Float.parseFloat(properties.getProperty(prefix+"cnoMean"));
		if (properties.getProperty(prefix+"towOffset")!=null)    this.towOffset= Double.parseDouble(properties.getProperty(prefix+"towOffset"));
		if (properties.getProperty(prefix+"leapS")!=null)        this.leapS= Byte.parseByte(properties.getProperty(prefix+"leapS"));
		if (properties.getProperty(prefix+"satsUsed")!=null)     this.satsUsed= Byte.parseByte(properties.getProperty(prefix+"satsUsed"));
		if (properties.getProperty(prefix+"cnoMeanSigma")!=null) this.cnoMeanSigma= Byte.parseByte(properties.getProperty(prefix+"cnoMeanSigma"));
		if (properties.getProperty(prefix+"reserved")!=null)     this.reserved= Byte.parseByte(properties.getProperty(prefix+"reserved"));
		
	}
 	
	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+"timeOfWeekMs", this.timeOfWeekMs+"");
		properties.setProperty(prefix+"status",       this.status+"");
		properties.setProperty(prefix+"ecef",         IntersceneMatchParameters.doublesToString(this.ecef));
		properties.setProperty(prefix+"lla",          IntersceneMatchParameters.doublesToString(this.lla));
		properties.setProperty(prefix+"hMSL",         this.hMSL+"");
		properties.setProperty(prefix+"hAcc",         this.hAcc+"");
		properties.setProperty(prefix+"vAcc",         this.vAcc+"");
		properties.setProperty(prefix+"pDop",         this.pDop+"");
		properties.setProperty(prefix+"cnoMean",      this.cnoMean+"");
		properties.setProperty(prefix+"towOffset",    this.towOffset+"");
		properties.setProperty(prefix+"leapS",        this.leapS+"");
		properties.setProperty(prefix+"satsUsed",     this.satsUsed+"");
		properties.setProperty(prefix+"cnoMeanSigma", this.cnoMeanSigma+"");
		properties.setProperty(prefix+"reserved",     this.reserved+"");
		return properties;
	}

}