package com.elphel.imagej.ims;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;

public class Did_pimu {
	/** 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 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 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;
	}

}