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;
	/** 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
	/** 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
	public static int eInsStatusFlags_mask = 0x8800596e; // 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
	
	
	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 ((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] <-90) || (lla[1] > 90) || // 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]) ;
    	}
	}
	
	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);
	}
	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));		
		return properties;
	}
}