/**
 **
 ** Did_pimu.java - InertialSense DID_PIMU message decoder for pre-integrated IMU data
 **
 ** Copyright (C) 2023-2024 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  Did_pimu.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;

public class Did_pimu {
	public static final int IMU_STATUS_IMU_OK_MASK = 0x003F0000; // lower bits may have error
	public static final int IMU_STATUS_SATURATION_MASK = 0x0000003F;
	public static final int IMU_STATUS_ZEROS = 0xffc0f800;
	public static final double IMU_MAX_THETA = 10.0;   //rad/s
	public static final double IMU_MAX_VEL =   20.0;   //m/s/s
	public static final double IMU_MIN_PER =    0.008; // s
	public static final double IMU_MAX_PER =    1.0;   // s
	
	/** Time since boot up in seconds.  Convert to GPS time of week by adding gps.towOffset */
	public double    time;
	/** Integral period in seconds for delta theta and delta velocity.  This is configured using DID_FLASH_CONFIG.startupNavDtMs. */
	public float	 dt;
	/** IMU Status (eImuStatus) */
	public int       status;
	/** IMU delta theta (gyroscope {p,q,r} integral) in radians in sensor frame */
	public float []  theta = new float [3];
	/** IMU delta velocity (accelerometer {x,y,z} integral) in m/s in sensor frame */
	public float []  vel = new float[3];
	
	public Did_pimu (ByteBuffer bb) {
		bb.order(ByteOrder.LITTLE_ENDIAN);		
		time =     bb.getDouble();
		dt =       bb.getFloat();
		status =   bb.getInt();
		theta[0]=  bb.getFloat();  theta[1] = bb.getFloat();  theta[2]= bb.getFloat();
		vel[0]=    bb.getFloat();  vel[1] = bb.getFloat();    vel[2]= bb.getFloat();
	}
	public Did_pimu() {}

	public Did_pimu(String prefix, Properties properties) {
		getProperties(prefix, properties);
	}
	
	public Did_pimu interpolate(double frac, Did_pimu next_did) {
		Did_pimu new_did = new Did_pimu();
		new_did.time = Did_ins.interpolateDouble(frac, time, next_did.time);
		new_did.dt = dt;
		new_did.status = status;
    	for (int i = 0; i < theta.length; i++) {
    		new_did.theta[i] =   Did_ins.interpolateFloat(frac, theta[i], next_did.theta[i]) ;
    		new_did.vel[i] =     Did_ins.interpolateFloat(frac, vel[i],   next_did.vel[i]) ;
    	}
		return new_did;
	}
	
	public boolean isDidSane() {
		if (((status ^ IMU_STATUS_IMU_OK_MASK) &  IMU_STATUS_IMU_OK_MASK) != 0) {
			System.out.println("Did_pimu.isDidSane(): not OK status = "+String.format("0x%08x", status));
			return false;
		}
		if ((status & IMU_STATUS_SATURATION_MASK) != 0) {
			System.out.println("Did_pimu.isDidSane(): saturated status = "+String.format("0x%08x", status));
			return false;
		}
		if ((status & IMU_STATUS_ZEROS) != 0) {
			System.out.println("Did_pimu.isDidSane(): garbage status = "+String.format("0x%08x", status));
			return false;
		}
		if (dt < IMU_MIN_PER) {
			System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s < "+IMU_MIN_PER+" s");
			return false;
		}
		if (dt > IMU_MAX_PER) {
			System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s > "+IMU_MAX_PER+" s");
			return false;
		}
		for (int i = 0; i < 3; i++) {
			if (Math.abs(theta[i]) > IMU_MAX_THETA) {
				System.out.println("Did_pimu.isDidSane(): insane theta ["+i+"]= "+theta[i]+" rad/s");
				return false;
			}
			if (Math.abs(vel[i]) > IMU_MAX_VEL) {
				System.out.println("Did_pimu.isDidSane(): insane vel ["+i+"]= "+vel[i]+" m/s/s");
				return false;
			}
		}
		return true;
	}
	
	public int pack(ByteBuffer bb) {
		int p_start = bb.position();
		bb.putDouble(time);
		bb.putFloat(dt);
		bb.putInt(status);
		bb.putFloat(theta[0]); bb.putFloat(theta[1]); bb.putFloat(theta[2]);
		bb.putFloat(vel[0]);   bb.putFloat(vel[1]);   bb.putFloat(vel[2]);
		int p_end = bb.position();
		return p_end - p_start;
	}
	
	public String toString() {
		String s = "DID_PIMU\n";
		s += String.format("time:       %f\n", time); 
		s += String.format("dt:         %f\n", dt); 
		s += String.format("status:     0x%x\n", status); 
		s += String.format("theta:      %f, %f, %f\n", theta[0], theta[1], theta[2]); 
		s += String.format("vel:        %f, %f, %f",   vel[0], vel[1], vel[2]); 
		return s;
	}
	
	public void getProperties(String prefix, Properties properties) {
		if (properties.getProperty(prefix+"time")!=null)       this.time=   Double.parseDouble(properties.getProperty(prefix+"time"));
		if (properties.getProperty(prefix+"dt")!=null)         this.dt=     Float.parseFloat(properties.getProperty(prefix+"dt"));
		if (properties.getProperty(prefix+"status")!=null)     this.status= Integer.parseInt(properties.getProperty(prefix+"status"));
		if (properties.getProperty(prefix+"theta")!=null) 	   this.theta=  IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"theta"),3);
		if (properties.getProperty(prefix+"vel")!=null) 	   this.vel=    IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"vel"),3);
	}

	public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
		if (properties == null) {
			properties = new Properties();
		}
		properties.setProperty(prefix+"time",     this.time+"");
		properties.setProperty(prefix+"dt",       this.dt+"");
		properties.setProperty(prefix+"status",   this.status+"");
		properties.setProperty(prefix+"theta",    IntersceneMatchParameters.floatsToString(this.theta));
		properties.setProperty(prefix+"vel",      IntersceneMatchParameters.floatsToString(this.vel));
		return properties;
	}
	
}