package com.elphel.imagej.ims;

import java.awt.Point;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashSet;
import java.util.Set;

import com.elphel.imagej.cameras.CLTParameters;


public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
	public static final int REC_SIZE =           64;
	// bits 24.27 of local usec
	public static final int REC_TYPE_IMU =        0; // not used
	public static final int REC_TYPE_GPS =        1; // now IMX-5 also
	public static final int REC_TYPE_IMG =        2; // local image or external trigger TS
	public static final int REC_TYPE_ODO =        3; // "Odometer" or any other external event
	// bits 28.31 of local usec
	public static final int REC_SUBTYPE_IMX5 =    8; // lower bits - number of subpacket (8,9,10,..,15)
	public static final int REC_SUBPACKET_IMX5 =  7; // bit mask for DID subpacket number (0- first)
	
	
	public static final int LOCAL_USEC_OFFS =     0;
	public static final int LOCAL_SEC_OFFS =      4;
	public static final int MASTER_USEC_OFFS =    8;
	public static final int MASTER_SEC_OFFS =    12;
	public static final int IMX5_DID =            8;
	public static final int IMX5_LENGTH =        10;
	public static final int IMX5_PAYLOAD_FIRST = 12;
	public static final int IMX5_PAYLOAD_NEXT =   8;
	String abs_path;
	int      num_recs; 
	int      num_subs = 0; // multiply type[0] to combine subtypes with types
	int  []  type_lut;
	int [][] rec_types;          // [][0] - 1 byte with swapped nibbles - MSB of microseconds  (byte 3 of each 64-byte record),
	                             // [][1] - image channel or IMX5 DID                        
	int [][] rec_first_last;     // indexed by rec_types, {first_index_in_file, last_index_in_file}
	double [][] first_last_ts;   // {first, last} master timestamps for each record type
	double local_lag =     0.0;  // local timestamp lag to master camera corresponding to the first record in a file) 
	double local_rel_lag = 1.0;  // local timestamp lag correction scale local_lag by local_rel_lag * master_ts_difference
	double ts_local_start;
	double ts_master_start;      // master (image) timestamp in seconds, corresponding to the first record in this file
	double ts_master_end;        // master (image) timestamp in seconds, corresponding to the last  record in this file
	int [] gnss_start = null;    // gnss time (in weeks, ms) corresponding to the first record in this file
	int [] gnss_end =   null;    // gnss time (in weeks, ms) corresponding to the last  record in this file 
    double gnss_frac_start = Double.NaN; // Fractional seconds of GNSS 1pps in master clock seconds corresponding to the first record in this file
    double gnss_frac_end =  Double.NaN; // Fractional seconds of GNSS 1pps in master clock seconds corresponding to the last  record in this file 
/// -0.5 <= gnss_frac_start < 0.5, abs (gnss_frac_end - gnss_frac_start) < 0.5 while abs (gnss_frac_end) may be > 0.5 !	

    ByteBuffer bb;
    boolean [] has_ins = {false, false}; // has Did_ins_1, has Did_ins_1;
    float [][] ins_quats =      null;// fixing missing Did_ins_2 records. Restore from Euler angles in Did_ins_1, requires to process all consecutive records
    
    public boolean hasIns(boolean ins2) {
    	return has_ins[ins2? 1 : 0];
    }
    void setHasIns(boolean ins2, boolean has) {
    	has_ins[ins2? 1 : 0] = has;
    }
	// add time-conversion methods here?
	public void open() throws IOException{
		if (bb == null) {
			byte[] allBytes = Files.readAllBytes(Paths.get(abs_path));
			bb = ByteBuffer.wrap(allBytes);
			bb.order(ByteOrder.LITTLE_ENDIAN);
		}
	}
	
	public void close() {
		bb = null;
	}
	
	public int getTypeIndex (
			int type,
			int did) {
		for (int i = 0; i < rec_types.length; i++) {
			if ((rec_types[i][0] == type) && (rec_types[i][1] == did)) {
				return i;
			}
		}
		return -1; // not found
	}
	
	public double [] getFirstLastTS(
			int type,
			int did) {
		int indx = getTypeIndex (type, did);
		return (indx >=0) ? first_last_ts[indx] : null;
	}
	
	public int [] getFirstLastIndex(
			int type,
			int did) {
		int indx = getTypeIndex (type, did);
		return (indx >=0) ? rec_first_last[indx] : null;
	}
	
	public int getLastBeforeIndex( // bb should be open
			double    ts_master,
			boolean   after, // false - before (including ==)
			int       type,
			int       did) {
		double [] first_last_ts = getFirstLastTS(type, did);
		if (first_last_ts == null) {
			return -1;
		}
		if (ts_master < first_last_ts[0]) {
			return -2;
		}
		if (ts_master > first_last_ts[1]) {
			 // timestamp later than last, return last (assuming next file start with larger timestamp)			
			return getFirstLastIndex(type, did)[1];
		}
		if (after) {
			if (ts_master == first_last_ts[1]) {
				return -3;
			}
		}
		int [] first_last_index = getFirstLastIndex(type, did); // should not be null as it matches getFirstLastTS()
		if (first_last_index[0] == first_last_index[1]) {
			return first_last_index[0]; // single choice
		}
		// initial approximation
		int nrec0 = first_last_index[0] + (int) Math.round((first_last_index[1]-first_last_index[0]) *
				(ts_master - first_last_ts[0])/ (first_last_ts[1]-first_last_ts[0]));
		// find any type in the opposite direction
		int drec = after ? -1: 1;
		int nrec = nrec0;
		search_opposite: {
			for (; (nrec > first_last_index[0]) && (nrec < first_last_index[1]); nrec += drec) {
				double ts_master_indx = getMasterTS(bb, nrec); 
				if (after?(ts_master_indx <= ts_master):(ts_master_indx > ts_master)) {
					// still need to check type, as master timestamp may be in wrong order
					// Timestamp is frame start for images
					int [] full_type = getFullType(bb, nrec);
					if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
						// is DID sane?
						if (isDidSane(nrec, did)) {
							break search_opposite; // return nrec;
						} else {
							System.out.println("getLastBeforeIndex()-1: Skipping insane DID="+did);
						}
					}
				}
			}
			System.out.println("getLastBeforeIndex(): could not find oppposite, ts_master="+ts_master+", after="+after);
			return first_last_index[after ? 1 : 0]; // could not find
		}
		drec = after ? 1: -1;
		for (; (nrec > first_last_index[0]) && (nrec < first_last_index[1]); nrec += drec) {
			double ts_master_indx = getMasterTS(bb, nrec); 
			if (after?(ts_master_indx > ts_master) :(ts_master_indx <= ts_master)) {
				int [] full_type = getFullType(bb, nrec);
				if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
					// is DID sane?
					if (isDidSane(nrec, did)) {
						return nrec;
					} else {
						System.out.println("getLastBeforeIndex()-2: Skipping insane DID="+did);
					}
				}
			}
		}
		return first_last_index[after ? 1 : 0]; 
	}
	
	public int getNextIndex( // assuming open
			int       indx,
			int       type,
			int       did) {
		int [] first_last_index = getFirstLastIndex(type, did); // should not be null as it matches getFirstLastTS()
		if (first_last_index == null) {
			return -1;
		}
		if (indx >= first_last_index[1]) {
			return -2;
		}
		for (int nrec = indx; nrec <= first_last_index[1]; nrec++) {
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
				return nrec;
			}
		}
		return -3; // should never get here. Make it throw?
	}

	
	public EventLoggerFileInfo (
			CLTParameters clt_parameters,
			String abs_path,
			boolean keep,
			String debug_path) throws IOException {
		int debugLevel = 0;
		this.abs_path = abs_path;
		System.out.println("EventLoggerFileInfo(): processing "+abs_path);
		open();
		num_recs = bb.array().length / REC_SIZE;
		Set <Point> types_set = new HashSet<Point>();
		for (int nrec = 0; nrec < num_recs; nrec++) {
			int [] full_type = getFullType(bb, nrec);
			if (full_type != null) {
				int before_size = types_set.size();
				types_set.add(new Point(full_type[0], full_type[1]));
				if ((debugLevel > -1) && (types_set.size() > before_size)) {
					System.out.println("EventLoggerFileInfo(): added "+full_type[0]+":"+full_type[1]+" @ nrec="+nrec);
				}
			}
		}
		ArrayList<Point> types_list = new ArrayList<Point>(types_set);
		Collections.sort(types_list, new Comparator<Point>() {
			@Override
			public int compare(Point lhs, Point rhs) { // ascending
				return (rhs.x > lhs.x) ? -1 : ((rhs.x < lhs.x) ? 1 : ( (rhs.y > lhs.y) ? 1 :( (rhs.y < lhs.y) ? -1 :0))) ;
			}
		});
		rec_types = new int [types_list.size()][2];
		num_subs = 0;
		int [] maxes = new int[rec_types[0].length];
		for (int nt = 0; nt < rec_types.length; nt++) {
			rec_types[nt][0] = 	types_list.get(nt).x;
			rec_types[nt][1] = 	types_list.get(nt).y;
			for (int i = 0; i < maxes.length; i++) {
				if (rec_types[nt][i] > maxes[i]) {
					maxes[i] = rec_types[nt][i]; 
				}
			}
		}
		num_subs = maxes[1] + 1;
		type_lut = new int [num_subs * (maxes[0] + 1)];
		Arrays.fill(type_lut, -1); // just to detect errors
		for (int nt = 0; nt < rec_types.length; nt++) {
			type_lut[rec_types[nt][0] * num_subs + rec_types[nt][1]] = nt;
		}
		
		processLocalToMaster(
				clt_parameters, // CLTParameters clt_parameters,
				debug_path,     // String debug_out_path,
				debugLevel
				); // false if no data
		// Timing calibration with DID_STROBE_IN_TIME
		// can be inaccurate if serial data from IMS has to wait. Also sometimes DID_STROBE_IN_TIME was stopping unexplained
		// if file contains DID_STROBE_IN_TIME, calculate absolute week an time of week in milliseconds
		if (debugLevel > -1) {
			System.out.println("calibrateFromStrobeInTime()");
		}
		calibrateFromStrobeInTime(debugLevel);
		
		if (debugLevel > -1) {
			System.out.println("calibrateFromDidIns(false)");
		}
		calibrateFromDidIns(false, debugLevel);

		if (debugLevel > -1) {
			System.out.println("calibrateFromDidIns(true)");
		}
		calibrateFromDidIns(true, debugLevel); // error
		
		// Timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)
		// Only fractional seconds can be calibrated, the record appears 0.1 sec after the event, local
		// timestamp corresponds to the actual leading edge of the 1pps pulse.
		calibrateFrom1Pps(debugLevel); // false if no data
		if (!keep) {
			close();
		}
		return;
	}
	
	
	/**
	 * Absolute timing calibration with DID_STROBE_IN_TIME
	 * can be inaccurate if serial data from IMS has to wait. Also sometimes DID_STROBE_IN_TIME was stopping unexplained
	 * if file contains DID_STROBE_IN_TIME, calculate absolute week an time of week in milliseconds
	 * 
	 * @param debugLevel - debug level
	 * @return false if no pps data is available, true if OK
	 * @throws IOException
	 */
	public boolean calibrateFromStrobeInTime(int debugLevel) throws IOException {
		// Absolute Timing calibration with DID_STROBE_IN_TIME
		// can be inacurate if serial data from IMS has to wait. Also sometimes DID_STROBE_IN_TIME was stopping unexplained
		// if file contains DID_STROBE_IN_TIME, calculate absolute week an time of week in milliseconds
		int type_imx5 = (REC_TYPE_GPS << 4) | REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		int indx_strobe_in_time = getTypeIndex (type_imx5, Imx5.DID_STROBE_IN_TIME);
		if (indx_strobe_in_time < 0) {
			System.out.println("calibrateFromStrobeInTime(): no DID_STROBE_IN_TIME available, aborting.");
			return false;
		}
		double [] first_last_ts =    getFirstLastTS(type_imx5, Imx5.DID_STROBE_IN_TIME); // master timestamps
		int []    first_last_index = getFirstLastIndex(type_imx5, Imx5.DID_STROBE_IN_TIME);
		if (first_last_ts[1] > first_last_ts[0]) {
			// Find all Did_strobe_in_time in the file, then preceding frame syncs
			// processing master timestamps to 
			double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
			int [] week_time0 = null;
			double ts_master0 = Double.NaN;

			for (int nrec = first_last_index[0]; nrec <= first_last_index[1]; nrec++) {
				int [] full_type = getFullType(bb, nrec);
				if ((full_type != null) && (full_type[0] == type_imx5) && (full_type[1] == Imx5.DID_STROBE_IN_TIME)) {
					// find previous image
					int nrec_img = nrec -1;
					for (; nrec_img >= 0; nrec_img--) {
						int [] full_type_img = getFullType(bb, nrec_img);
						if ((full_type_img != null) && (full_type_img[0] >> 4) == REC_TYPE_IMG) {
							// get master time
							double ts_master = getMasterTSImg(bb, nrec_img);
							// get gnss time
							byte [] payload = getDidPayload(
									null, // next_fileinfo, // may be null if payload does not extend beyond single record 
									nrec);
							if (payload == null) {
								System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
								break;
							}
							ByteBuffer bb_payload = ByteBuffer.wrap(payload);
							bb_payload.order(ByteOrder.LITTLE_ENDIAN);
							Did_strobe_in_time did_strobe_in_time = new Did_strobe_in_time (bb_payload);
							int [] week_time = did_strobe_in_time.getWeekTimeMs();
							double ts_gnss = 0.0;
							if (Double.isNaN(ts_master0)) {
								week_time0 =  week_time;
								ts_master0 = ts_master;
							}
							ts_gnss = Did_strobe_in_time.getWeekTimeMsDelta(week_time0, week_time);
							ts_master -= ts_master0;
							s0 +=  1;
							sx +=  ts_master;
							sx2 += ts_master * ts_master;
							sy +=  ts_gnss;
							sxy += ts_master * ts_gnss;
							if (debugLevel > 0) { // -1
								System.out.println (
										String.format("%6d %6d %10.6f %10.6f",nrec_img, nrec, ts_master, ts_gnss));
							}
							break;
						}
					}

				}

			}
			double d = (s0 * sx2 - sx * sx);
			double a = (sxy * s0 - sy * sx) / d;
			double b = (sx2 * sy - sx * sxy) / d;
			gnss_start = Did_strobe_in_time.getWeekTimeMs(
					week_time0, // int [] week_time_ms,
					b + (ts_master_start - ts_master0) * a);
			gnss_end = Did_strobe_in_time.getWeekTimeMs(
					week_time0, // int [] week_time_ms,
					b + (ts_master_end - ts_master0) * a);
			if (debugLevel > -1) {
				double offs_0 = 1694300600.0;
				System.out.println(String.format("gnss_start: week=%d, sec=%f, diff=%f",
						gnss_start[0], 0.001*gnss_start[1], (ts_master_start - 0.001*gnss_start[1] - offs_0)));
				System.out.println(String.format("gnss_end:   week=%d, sec=%f, diff=%f",
						gnss_end[0],   0.001*gnss_end[1], (ts_master_end - 0.001*gnss_end[1] - offs_0)));
			}
		}
		return true;
	}
	
	/**
	 * Absolute timing calibration with DID_INS_1 and DID_INS_2 
	 * Is inaccurate as serial data from IMS has to wait.
	 * Used only to find full seconds correspondence, fraction seconds will be found
	 * by calibrateFrom1Pps()
	 * @param type_ins_2 - true - use DID_INS_2, false - use DID_INS_1 
	 * @param debugLevel - debug level
	 * @return false if no data is available, true if OK
	 * @throws IOException
	 */

	public boolean calibrateFromDidIns(boolean type_ins_2, int debugLevel) throws IOException {
		int did_ins_type = type_ins_2 ? Imx5.DID_INS_2 : Imx5.DID_INS_1;
		int type_imx5 = (REC_TYPE_GPS << 4) | REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		int indx_did_ins = getTypeIndex (type_imx5, did_ins_type);
		if (indx_did_ins < 0){
			System.out.println("calibrateFromDidIns(): no DID_INS_"+(type_ins_2? 2 : 1)+" available, aborting.");
			setHasIns(type_ins_2, false);
			return false;
		}
		setHasIns(type_ins_2, true);
		double [] first_last_ts =    getFirstLastTS(type_imx5, did_ins_type); // master timestamps
		int []    first_last_index = getFirstLastIndex(type_imx5, did_ins_type);
		if (first_last_ts[1] > first_last_ts[0]) {
			// processing master timestamps to 
			double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
			int [] week_time0 = null;
			double ts_master0 = Double.NaN;
//			double full_time0 = Double.NaN;

			for (int nrec = first_last_index[0]; nrec <= first_last_index[1]; nrec++) {
				int [] full_type = getFullType(bb, nrec);
				if ((full_type != null) && (full_type[0] == type_imx5) && (full_type[1] == did_ins_type)) {
					double ts_master = getMasterTS(bb, nrec);
					/*
					if (nrec==58364) {
						System.out.println("nrec="+nrec+", type_ins_2="+type_ins_2); //*****
						System.out.println();
					}
					*/
					byte [] payload = getDidPayload(
							null, // next_fileinfo, // may be null if payload does not extend beyond single record 
							nrec);
					if (payload == null) {
						System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
						break;
					}
					ByteBuffer bb_payload = ByteBuffer.wrap(payload);
					bb_payload.order(ByteOrder.LITTLE_ENDIAN);
//					Did_ins did_ins = null;
					@SuppressWarnings("rawtypes")
					Did_ins did_ins = type_ins_2 ? (new Did_ins_2(bb_payload)) : (new Did_ins_1(bb_payload)); // error
					if (!did_ins.isDidSane()) {
						continue;
					}
//					double full_time = did_ins.getDoubleTime();
					int [] week_time = did_ins.getWeekTimeMs();
					if (week_time0 == null) {
						week_time0 =  week_time;
						ts_master0 = ts_master;
					}
					double ts_gnss = 0.0;
					ts_gnss = Did_strobe_in_time.getWeekTimeMsDelta(week_time0, week_time);
					ts_master -= ts_master0;
					s0 +=  1;
					sx +=  ts_master;
					sx2 += ts_master * ts_master;
					sy +=  ts_gnss;
					sxy += ts_master * ts_gnss;
					if (sy>2*sx) { // -1
						System.out.println (
								String.format("%6d %10.6f %10.6f", nrec, ts_master, ts_gnss)+" sx="+sx+" sy="+sy);
						System.out.println(Did_strobe_in_time.getWeekTimeMsDelta(week_time0, week_time));
					}
					
					if ((sy>2*sx) || (debugLevel > 0)) { // -1
						System.out.println (
								String.format("%6d %10.6f %10.6f", nrec, ts_master, ts_gnss)+" sx="+sx+" sy="+sy);
					}
				}
			}
			if (s0 == 0){
				System.out.println("calibrateFromDidIns(): no valid records available, aborting.");
				return false;
			}

			double d = (s0 * sx2 - sx * sx);
			double a = (sxy * s0 - sy * sx) / d;
			double b = (sx2 * sy - sx * sxy) / d;
			gnss_start = Did_strobe_in_time.getWeekTimeMs(
					week_time0, // int [] week_time_ms,
					b + (ts_master_start - ts_master0) * a);
			gnss_end = Did_strobe_in_time.getWeekTimeMs(
					week_time0, // int [] week_time_ms,
					b + (ts_master_end - ts_master0) * a);
			if (debugLevel > -1) {
				double offs_0 = 1694300600.0;
				System.out.println(String.format("gnss_did_start: week=%d, sec=%f, diff=%f",
						gnss_start[0], 0.001*gnss_start[1], (ts_master_start - 0.001*gnss_start[1] - offs_0)));
				System.out.println(String.format("gnss_did_end:   week=%d, sec=%f, diff=%f",
						gnss_end[0],   0.001*gnss_end[1], (ts_master_end - 0.001*gnss_end[1] - offs_0)));
				
			}
		}
		return true;
	}
	
	public boolean isDidSane(int nrec, int type) {
		//		int did_ins_type = type_ins_2 ? Imx5.DID_INS_2 : Imx5.DID_INS_1;
		byte[] payload;
		try {
			payload = getDidPayload(
					null, // next_fileinfo, // may be null if payload does not extend beyond single record 
					nrec);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			System.out.println("isDidSane(): failed to read record "+nrec);
			e.printStackTrace();
			return false;
		}
		if (payload == null) {
			System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
			return false;
		}
		ByteBuffer bb_payload = ByteBuffer.wrap(payload);
		bb_payload.order(ByteOrder.LITTLE_ENDIAN);

		switch (type) {
		case Imx5.DID_INS_1: 
			return (new Did_ins_1(bb_payload)).isDidSane();
		case Imx5.DID_INS_2: 
			return (new Did_ins_2(bb_payload)).isDidSane();
		case Imx5.DID_GPS1_POS: 
		case Imx5.DID_GPS2_POS: 
		case Imx5.DID_GPS1_UBX_POS: 
			return (new Did_gps_pos(bb_payload)).isDidSane();
		case Imx5.DID_PIMU: 
			return (new Did_pimu(bb_payload)).isDidSane();
		}
		return true;
	}
	
	/**
	 * Absolute timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)
	 * Only fractional seconds can be calibrated, the record appears 0.1 sec after the event, local
	 * timestamp corresponds to the actual leading edge of the 1pps pulse.
	 * 
	 * @param debugLevel - debug level
	 * @return false if no pps data is available, true if OK
	 */
	public boolean calibrateFrom1Pps(int debugLevel) {
		int type_1pps = REC_TYPE_ODO << 4;
		int indx_1pps = getTypeIndex (type_1pps, 0);
		if (indx_1pps < 0) {
			System.out.println("calibrateFrom1Pps(): no , aborting.");
			return false;
		}
		double [] first_last_ts =    getFirstLastTS(type_1pps, 0); // master timestamps
		int []    first_last_index = getFirstLastIndex(type_1pps, 0);
		if (first_last_ts[1] > first_last_ts[0]) { // has >=2 records of type REC_TYPE_ODO
			double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
			double ts_local0 = Double.NaN;
			double ts_frac0 =  Double.NaN;
			for (int nrec = first_last_index[0]; nrec <= first_last_index[1]; nrec++) {
				int [] full_type = getFullType(bb, nrec);
				if ((full_type != null) && (full_type[0] == type_1pps)) {
					double ts_local =  getLocalTS(bb, nrec);
					if (Double.isNaN(ts_local0)) {
						ts_local0 =  ts_local;
						ts_frac0 =   ts_local - Math.round(ts_local);
					}
					ts_local -= ts_local0;
					double ts_frac = ts_local - Math.round(ts_local);
					s0 +=  1;
					sx +=  ts_local;
					sx2 += ts_local * ts_local;
					sy +=  ts_frac;
					sxy += ts_local * ts_frac;
					if (debugLevel > 0) {
						System.out.println (
								String.format("%6d %10.6f %10.6f", nrec, ts_local, ts_frac));
					}
				}
			}
			double d = (s0 * sx2 - sx * sx);
			double a = (sxy * s0 - sy * sx) / d;
			double b = (sx2 * sy - sx * sxy) / d;
			double ts_local_end =   getLocalTS(bb, num_recs - 1);
			double local_frac_start = ts_frac0 + b  + (ts_local_start - ts_local0) * a; 
			double local_frac_end =   ts_frac0 + b +  (ts_local_end -   ts_local0) * a;
			double master_frac_start = local_frac_start+ (ts_local_start - ts_master_start);
			double master_frac_end =   local_frac_end+   (ts_local_end -   ts_master_end);
			gnss_frac_start = master_frac_start - Math.round(master_frac_start);
			gnss_frac_end =   master_frac_end -   Math.round(master_frac_start);
			
			if (debugLevel > -1) {
				System.out.println(String.format("gnss_frac_start: %f", gnss_frac_start));
				System.out.println(String.format("gnss_frac_end:   %f", gnss_frac_end));
			}
		}			
		return true;
	}
	
	/**
	 * processing local to master timestamps correspondence
	 * @param debug_out_path - null or file path to save master/local TS matching
	 * @param debugLevel - debug level
	 * @throws IOException 
	 */
	public void processLocalToMaster(
			CLTParameters clt_parameters,			
			String debug_out_path, 
			int debugLevel) throws IOException {
		boolean ims_manual = (clt_parameters != null) && clt_parameters.imp.ims_manual;
		double  ims_local_ts =     ims_manual ? clt_parameters.imp.ims_local_ts :     Double.NaN;
		double  ims_master_ts =    ims_manual ? clt_parameters.imp.ims_master_ts :    Double.NaN;
		double  ims_master_scale = ims_manual ? clt_parameters.imp.ims_master_scale : Double.NaN;
		rec_first_last = new int [rec_types.length][];
		double ts_local0 =  Double.NaN;
		double ts_master0 = Double.NaN;
		double s0=0, sx = 0, sx2 = 0, sy=0, sxy=0;
		int nimg = 0;
		for (int nrec = 0; nrec < num_recs; nrec++) {
			int [] full_type = getFullType(bb, nrec);
			if (full_type != null) {
				int ti = type_lut [full_type[0] * num_subs + full_type[1]];
				if (rec_first_last[ti] == null) {
					rec_first_last[ti] = new int [] {nrec,nrec};
				} else {
					rec_first_last[ti][1] = nrec;
				}
				if ((full_type[0] >> 4) == REC_TYPE_IMG) {
					double ts_local =  getLocalTS(bb, nrec);
					double ts_master = getMasterTSImg(bb, nrec);
					if (debugLevel > 0) { // -1
						System.out.println (
								String.format("%6d %6d %10.6f",nimg, nrec, ts_master));
					}
					nimg++;
					if (Double.isNaN(ts_local0)) { // first image - sample local and master
						ts_local0 =  ts_local;
						ts_master0 = ts_master;
					}
					ts_local -=  ts_local0;
					ts_master -= ts_master0;
					s0 +=  1;
					sx +=  ts_master;
					sx2 += ts_master * ts_master;
					sy +=  ts_local;
					sxy += ts_master * ts_local;
				}
				// just print time and gps:
				int type_imx5 = (REC_TYPE_GPS << 4) | REC_SUBTYPE_IMX5; // 0x18 - first subpacket
				if ((full_type[0] >> 4) == type_imx5) {
				}
			}
		}
		ts_local_start =        getLocalTS(bb, 0);
		double ts_local_end =   getLocalTS(bb, num_recs - 1);
		if (ims_manual) {
			ts_local0 =  ims_local_ts;
			ts_master0 = ims_master_ts;
			double ts_local =  1.0;
			double ts_master = ts_local / (1 - ims_master_scale);
			sx +=  ts_master;
			sx2 += ts_master * ts_master;
			sy +=  ts_local;
			sxy += ts_master * ts_local;
			s0 = 2;
		}			
		double d = (s0 * sx2 - sx * sx);
		double a = (sxy * s0 - sy * sx) / d;
		double b = (sx2 * sy - sx * sxy) / d;

			/*
			double dlocal0 =  ts_local_start - ims_local_ts ;
			double dmaster0 = dlocal0/(1 - ims_master_scale);
			ts_master_start = ims_master_ts + dmaster0;
			double dlocal1 =  ts_local_end - ims_local_ts ;
			double dmaster1 = dlocal1/(1 - ims_master_scale);
			ts_master_end =   ims_master_ts + dmaster1;
//			ts_master0 = 
			a= (ts_local_end - ts_local_start) / (ts_master_end - ts_master_start);
			
		} else {
			*/
		local_rel_lag = a;
		ts_master_start = ts_master0 + (ts_local_start - ts_local0 - b) / local_rel_lag;
		local_lag =       ts_local_start - ts_master_start;
		ts_master_end = ts_master_start + (ts_local_end - ts_local_start) / local_rel_lag;
			
//		}
		
		// calculate offsets and timestamp range (master?)
		first_last_ts = new double [rec_first_last.length][2];
		for (int i = 0; i < rec_first_last.length; i++) {
			for (int j = 0; j < rec_first_last[i].length; j++) {
				double ts_local =  getLocalTS(bb, rec_first_last[i][j]);
				first_last_ts[i][j] = ts_master_start + (ts_local - ts_local_start) / local_rel_lag ;
			}
		}
		if (debug_out_path != null) {
			debug_out_path = Paths.get(debug_out_path).normalize().toString();
			Path op = Paths.get(debug_out_path);
			op=op.normalize();
			(new File(op.getParent().toString())).mkdirs();
			nimg = 0;
			FileWriter writer = new FileWriter (debug_out_path);
			String header =
					"image\t"+
			        "nrec\t" +
			        "local TS\t"+
			        "master TS\t"+
			        "predicted master\t"+
			        "prediction error\n"; 
			writer.write(header);
			
			for (int nrec = 0; nrec < num_recs; nrec++) {
				int [] full_type = getFullType(bb, nrec);
				if ((full_type != null) && ((full_type[0] >> 4) == REC_TYPE_IMG)) {
					double ts_local =  getLocalTS(bb, nrec);
					double ts_master = getMasterTSImg(bb, nrec);
					double ts_master_calc = getMasterTS(ts_local);
					writer.write(
							String.format("%6d\t%6d\t%10.6f\t%10.6f\t%10.6f\t%10.6f\n",
									nimg, nrec, ts_local, ts_master, ts_master_calc, ts_master-ts_master_calc));
					nimg++;
				}
			}
			writer.close();
		}
		return;
	}	
	
	/**
	 * Calculate quaternions normally available in Did_ins_2 and store them in this.ins_quats 
	 * @throws IOException
	 */
	public void setInsQuats() throws IOException {
		if (ins_quats != null) {
			return; // already calculated
		}
		ins_quats = new float [num_recs][];
		int dbg_nrec = -5000;
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		float [] prev_q = null; // first time will have arbitrary sign
		open();
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("setInsQuats(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				Did_ins_1 did_ins_1 = new Did_ins_1(bb_payload);
				if ((did_ins_1.uvw[0] == 0) && (did_ins_1.uvw[1] == 0) && (did_ins_1.uvw[2] == 0)) {
					continue;
				}
				
				ins_quats[nrec] = Did_ins.eulerToQuat(
						did_ins_1.theta, // double [] rpy,
						prev_q); // double [] prev_q);
				prev_q = ins_quats[nrec];
			}
		}
		return;
	}
	
	public float [] getQuat(int nrec) {
		return ins_quats[nrec];
	}
	
	
	public void listDidIns1(String path, boolean keep, double [] vert_xyz) throws IOException {
		double max_offset = 1000; // m
		open();
		setInsQuats();
		float [] start_ned = null;
		FileWriter writer = null;
		if (path != null) {
			writer = new FileWriter (path);
		}
		String header = "record\t"+
		        "master TS\t"+
		        "local TS\t"+
		        "timeOfWeek\t" +
		        "insStat\t"+
		        "hdwStat\t"+
		        "theta-0\ttheta-1\ttheta-2\t"+
		        
		        "qn2b0\tqn2b1\tqn2b2\tqn2b3\t"+ // adding calculated quaternions

		        "u\tv\tw\t"+
		        "lat\tlong\talt\t"+
		        "ned:N\tned:E\tned:D"; 
		if (vert_xyz != null) {
			header +="\tvert:X\tvert:Y\tvert:Z";
		}
		header +="\n";
		if (writer != null) {
			writer.write(header);
		} else {
			System.out.print(header);
		}
		int dbg_nrec = -5000;
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("listDidIns1(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				Did_ins_1 did_ins_1 = new Did_ins_1(bb_payload);
				if ((did_ins_1.uvw[0] == 0) && (did_ins_1.uvw[1] == 0) && (did_ins_1.uvw[2] == 0)) {
					continue;
				}
				if ((start_ned != null) &&
						(       (Math.abs(start_ned[0] - did_ins_1.ned[0]) > max_offset) ||
								(Math.abs(start_ned[1] - did_ins_1.ned[1]) > max_offset) ||
								(Math.abs(start_ned[2] - did_ins_1.ned[2]) > max_offset)
								)){
					start_ned = null;
				}
				if (start_ned == null) {
					start_ned = did_ins_1.ned.clone();
				}
				start_ned = new float[] {0,0,0};
				String line =String.format(
						"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t"+
						"%8.4f\t%8.4f\t%8.4f\t%8.4f\t"+
						"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f",
						nrec,
						getMasterTS(nrec),
						getLocalTS(nrec),
						did_ins_1.timeOfWeek,
						did_ins_1.insStatus,
						did_ins_1.hdwStatus,
						did_ins_1.theta[0], did_ins_1.theta[1], did_ins_1.theta[2], // Euler angles: roll, pitch, yaw in radians with respect to NED
						ins_quats[nrec][0],ins_quats[nrec][1],ins_quats[nrec][2],ins_quats[nrec][3],
						did_ins_1.uvw[0], did_ins_1.uvw[1], did_ins_1.uvw[2],       // Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
						did_ins_1.lla[0], did_ins_1.lla[1], did_ins_1.lla[2],       //  WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
						did_ins_1.ned[0] - start_ned[0], did_ins_1.ned[1] - start_ned[1], did_ins_1.ned[2] - start_ned[2]        //  North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude
						);
				if (vert_xyz != null) {
					double [] quat = {ins_quats[nrec][0],ins_quats[nrec][1],ins_quats[nrec][2],ins_quats[nrec][3]};
					double [][] R = QuatVertLMA.quaternionToRotationMatrix(quat);
					double [] ims_down = new double[3];
					for (int i = 0; i < 3; i++) {
						for (int k = 0; k < 3; k++) {
							ims_down[i] += R[i][k]*vert_xyz[k];
						}
					}
					line +=String.format("\t%13.10f\t%13.10f\t%13.10f",ims_down[0],ims_down[1],ims_down[2]);
				}
				line += "\n";
				if (writer != null) {
					writer.write(line);
				} else {
					System.out.print(line);
				}
			}
		}
		if (writer != null) {
			writer.close();
		}
		if (!keep) {
			close();
		}
		return;
	}

	public void listDidIns2(String path, boolean keep) throws IOException {
		open();
		FileWriter writer = null;
		if (path != null) {
			writer = new FileWriter (path);
		}
		String header = "record\t"+
		        "master TS\t"+
		        "local TS\t"+
		        "timeOfWeek\t" +
		        "insStat\t"+
		        "hdwStat\t"+
		        "qn2b0\tqn2b1\tqn2b2\tqn2b3\t"+
		        "u\tv\tw\t"+
		        "lat\tlong\talt\n"; 
		if (writer != null) {
			writer.write(header);
		} else {
			System.out.print(header);
		}
		int dbg_nrec = -5000;
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("listDidIns2(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_2)) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				Did_ins_2 did_ins_2 = new Did_ins_2(bb_payload);
				if ((did_ins_2.uvw[0] == 0) && (did_ins_2.uvw[1] == 0) && (did_ins_2.uvw[2] == 0)) {
					continue;
				}
				String line =String.format(
						"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t"+
				        "%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\n",
						nrec,
						getMasterTS(nrec),
						getLocalTS(nrec),
						did_ins_2.timeOfWeek,
						did_ins_2.insStatus,
						did_ins_2.hdwStatus,
						did_ins_2.qn2b[0], did_ins_2.qn2b[1], did_ins_2.qn2b[2], did_ins_2.qn2b[3], // Quaternion body rotation with respect to NED: W, X, Y, Z
						did_ins_2.uvw[0], did_ins_2.uvw[1], did_ins_2.uvw[2],                       // Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
						did_ins_2.lla[0], did_ins_2.lla[1], did_ins_2.lla[2]                        //  WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
						);
				if (writer != null) {
					writer.write(line);
				} else {
					System.out.print(line);
				}
			}
		}
		if (writer != null) {
			writer.close();
		}
		if (!keep) {
			close();
		}
	}
	
	
	public double [][] getQuats(
			double ts_start, // or NaN
			double ts_end, // or NaN
			boolean keep) throws IOException {
		class Double4 {
			double [] quat = new double[4];
			Double4 (float [] fdata){
				double [] data = new double[fdata.length];
				for (int i = 0; i < data.length; i++) {
					data[i] = fdata[i];
				}
			}
		}
		ArrayList<Double4> quat_list = new ArrayList<Double4>();
		open();
		int dbg_nrec = -5000;
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("listDidIns2(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_2)) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				Did_ins_2 did_ins_2 = new Did_ins_2(bb_payload);
				if ((did_ins_2.uvw[0] == 0) && (did_ins_2.uvw[1] == 0) && (did_ins_2.uvw[2] == 0)) {
					continue;
				}
				double ts = getMasterTS(nrec);
				if (!(ts < ts_start) && !(ts > ts_end)) {
					quat_list.add(new Double4(did_ins_2.qn2b));
				}
			}
		}
		if (!keep) {
			close();
		}
		double [][] quats = new double[quat_list.size()][];
		for (int i = 0; i < quats.length; i++) {
			quats[i] = quat_list.get(i).quat;
		}
		return quats;
	}
	
	
	
	public double [][] getQuatsIns1(
			double ts_start, // or NaN
			double ts_end, // or NaN
			boolean keep) throws IOException {
		class Double4 {
			double [] quat;
			Double4 (float [] fdata){
				quat = new double[fdata.length];
				for (int i = 0; i < quat.length; i++) {
					quat[i] = fdata[i];
				}
				return;
			}
		}
		ArrayList<Double4> quat_list = new ArrayList<Double4>();
		double max_offset = 1000; // m
		open();
		setInsQuats();
		float [] start_ned = null;
		int dbg_nrec = -5000;
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("getQuatsIns1(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				Did_ins_1 did_ins_1 = new Did_ins_1(bb_payload);
				if ((did_ins_1.uvw[0] == 0) && (did_ins_1.uvw[1] == 0) && (did_ins_1.uvw[2] == 0)) {
					continue;
				}
				if ((start_ned != null) &&
						(       (Math.abs(start_ned[0] - did_ins_1.ned[0]) > max_offset) ||
								(Math.abs(start_ned[1] - did_ins_1.ned[1]) > max_offset) ||
								(Math.abs(start_ned[2] - did_ins_1.ned[2]) > max_offset)
								)){
					start_ned = null;
				}
				if (start_ned == null) {
					start_ned = did_ins_1.ned.clone();
				}
				start_ned = new float[] {0,0,0};
				
				double ts = getMasterTS(nrec);
				if (!(ts < ts_start) && !(ts > ts_end)) {
					quat_list.add(new Double4(ins_quats[nrec]));
				}
			}
		}
		if (!keep) {
			close();
		}
		double [][] quats = new double[quat_list.size()][];
		for (int i = 0; i < quats.length; i++) {
			quats[i] = quat_list.get(i).quat;
		}
		return quats;
	}
	
	public void listGPS (String path, int gps_mask, boolean keep) throws IOException {
		// +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
//		double max_offset = 1000; // m
		open();
//		float [] start_ned = null;
		FileWriter writer = null;
		if (path != null) {
			writer = new FileWriter (path);
		}
		String header = "record\t"+
		        "     master TS\t"+
		        "     local TS\t"+
		        "timeOfWeek\t" +
		        "did\t"+
		        "status\t"+
		        "ecef-X\tecef-Y\tecef-Z\t"+
		        "lat\tlong\talt\t"+
		        "hMSL\t"+ // 7.2f
		        "hAcc\t"+ // 7.3f
		        "vAcc\t"+ // 7.3f
		        "pDop\t"+ // 7.3f
		        "cnoMean\t"+ // 7.3f
		        "towOffset\t"+ // 11.4f
		        "lS\t" + // 3d
		        "sat\t" + // 3d
		        "sgm\t" + // 3d
		        "rsv\n";// 3d 
		if (writer != null) {
			writer.write(header);
		} else {
			System.out.print(header);
		}

		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		for (int nrec = 0; nrec < num_recs; nrec++) {
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) &&
					(
					(((gps_mask & 1) != 0) && (full_type[1] == Imx5.DID_GPS1_POS)) ||
					(((gps_mask & 2) != 0) && (full_type[1] == Imx5.DID_GPS2_POS)) ||
					(((gps_mask & 4) != 0) && (full_type[1] == Imx5.DID_GPS1_UBX_POS))
					)
					) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				
				Did_gps_pos did_gps_pos = new Did_gps_pos(bb_payload);
				if ((did_gps_pos.lla[0] == 0) && (did_gps_pos.lla[1] == 0) && (did_gps_pos.lla[2] == 0)) {
					continue;
				}
				String line =String.format(
						"%6d\t%17.6f\t%17.6f\t%10.3f\t%3d\t%08x\t%13.4f\t%13.4f\t%13.4f\t"+
				        "%12.7f\t%12.7f\t%12.7f\t%7.2f\t%7.3f\t%7.3f\t%7.3f\t%7.3f\t%11.4f\t"+
						"%3d\t%3d\t%3d\t%3d\n",
						nrec,
						getMasterTS(nrec),
						getLocalTS(nrec),
						0.001 * did_gps_pos.timeOfWeekMs,
						full_type[1], // DID
						did_gps_pos.status,
						did_gps_pos.ecef[0], did_gps_pos.ecef[1], did_gps_pos.ecef[2], // Position in ECEF {x,y,z} (m)
						did_gps_pos.lla[0], did_gps_pos.lla[1], did_gps_pos.lla[2],       //  WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
						did_gps_pos.hMSL, did_gps_pos.hAcc, did_gps_pos.vAcc, did_gps_pos.pDop, did_gps_pos.cnoMean,
						did_gps_pos.towOffset,
						did_gps_pos.leapS, did_gps_pos.satsUsed, did_gps_pos.cnoMeanSigma, did_gps_pos.reserved
						);
				if (writer != null) {
					writer.write(line);
				} else {
					System.out.print(line);
				}
			}
		}
		if (writer != null) {
			writer.close();
		}
		if (!keep) {
			close();
		}
	}
	
	
	public void listPimu(String path, boolean keep) throws IOException {
		open();
		FileWriter writer = null;
		if (path != null) {
			writer = new FileWriter (path);
		}
		String header = "record\t"+
		        "master TS\t"+
		        "local TS\t"+
		        "time since boot\t" +
		        "dt\t"+
		        "status\t"+
		        "omega-X (rad/s)\tomega-Y (rad/s)\tomega-Z (rad/s)\t"+
		        "a-X (m/s^2)\ta-Y (m/s^2)\ta-Z (m/s^2)\n"; 
		if (writer != null) {
			writer.write(header);
		} else {
			System.out.print(header);
		}
		int dbg_nrec = -5000;
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("listPimu(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_PIMU)) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				Did_pimu did_pimu = new Did_pimu(bb_payload);
				if ((did_pimu.vel[0] == 0) && (did_pimu.vel[1] == 0) && (did_pimu.vel[2] == 0)) {
					continue; // skip no-movement
				}
				String line =String.format(
						"%6d\t%17.6f\t%17.6f\t%17.6f\t%8.6f\t%08x\t%9.5f\t%9.5f\t%9.5f\t"+
				        "%9.4f\t%9.4f\t%9.4f\n",
						nrec,
						getMasterTS(nrec),
						getLocalTS(nrec),
						did_pimu.time,
						did_pimu.dt,
						did_pimu.status,
						did_pimu.theta[0]/did_pimu.dt, did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt, // Euler angles: roll, pitch, yaw in radians with respect to NED
						did_pimu.vel[0]/did_pimu.dt, did_pimu.vel[1]/did_pimu.dt, did_pimu.vel[2]/did_pimu.dt       // Velocity U, V, W in meters per second.  Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
						);
				if (writer != null) {
					writer.write(line);
				} else {
					System.out.print(line);
				}
			}
		}
		if (writer != null) {
			writer.close();
		}
		if (!keep) {
			close();
		}
	}
	
	public void listStrobeInTime(String path, boolean keep) throws IOException {
		open();
		FileWriter writer = null;
		if (path != null) {
			writer = new FileWriter (path);
		}
		String header = "record\t"+
		        "master TS\t"+
		        "local TS\t"+
		        "week\t" +
		        "timeOfWeek\t" +
		        "pin\t"+
		        "count"+"\n"; 
		if (writer != null) {
			writer.write(header);
		} else {
			System.out.print(header);
		}
		int dbg_nrec = -5000;
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("listStrobeInTime(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_STROBE_IN_TIME)) {
				byte [] payload = getDidPayload(
						null, // next_fileinfo, // may be null if payload does not extend beyond single record 
						nrec);
				if (payload == null) {
					System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
					break;
				}
				ByteBuffer bb_payload = ByteBuffer.wrap(payload);
				bb_payload.order(ByteOrder.LITTLE_ENDIAN);
				Did_strobe_in_time did_strobe_in_time = new Did_strobe_in_time(bb_payload);
				if (did_strobe_in_time.week == 0) {
					continue; // skip no-movement
				}
				String line =String.format(
						"%6d\t%17.6f\t%17.6f\t%7d\t%10.3f\t%3d\t%7d\n",
						nrec,
						getMasterTS(nrec),
						getLocalTS(nrec),
						did_strobe_in_time.week,
						0.001 * did_strobe_in_time.timeOfWeekMs,
						did_strobe_in_time.pin,
						did_strobe_in_time.count);
				if (writer != null) {
					writer.write(line);
				} else {
					System.out.print(line);
				}
			}
		}
		if (writer != null) {
			writer.close();
		}
		if (!keep) {
			close();
		}
	}
	
	public void list1Pps(String path, boolean keep) throws IOException {
		open();
		FileWriter writer = null;
		if (path != null) {
			writer = new FileWriter (path);
		}
		String header = "record\t"+
		        "master TS\t"+
		        "local TS\t"+
		        "master frac\t"+
		        "local frac\t" +
		        "ms\t" +
		        "ms-master\t"+
		        "frac ms-master\t"+
		        "tow_corr\n"; 

		if (writer != null) {
			writer.write(header);
		} else {
			System.out.print(header);
		}
		int dbg_nrec = -5000;
		int type_1pps = EventLoggerFileInfo.REC_TYPE_ODO << 4; // 0x30
		for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
				System.out.println("list1Pps(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type_1pps)) {
				double local_ts = getLocalTS(nrec);
				double master_ts = getMasterTS(nrec);
//				double ms = 0.001* gnss_start[1] + (0.001*(gnss_end[1]-gnss_start[1])*nrec)/num_recs;
				if (gnss_start==null) {
					System.out.println("list1Pps(): gnss_start==null, path="+path);
					break;
				}
				double ms = 0.001* (gnss_start[1] +
						(gnss_end[1]-gnss_start[1])*(master_ts-ts_master_start)/(ts_master_end-ts_master_start));
				String line =String.format(
						"%6d\t%17.6f\t%17.6f\t%9.6f\t%9.6f\t%17.6f\t%17.6f\t%9.6f\t%9.6f\n",
						nrec,
						master_ts,
						local_ts,
						master_ts-Math.round(master_ts),
						local_ts-Math.round(local_ts),
						ms,
						(ms-master_ts),
						(ms-master_ts)-Math.round(ms-master_ts),
						getTimeOfWeekCorrection(master_ts));
				if (writer != null) {
					writer.write(line);
				} else {
					System.out.print(line);
				}
			}
		}
		if (writer != null) {
			writer.close();
		}
		if (!keep) {
			close();
		}
	}
	
	
	
	
	
	byte [] getDidPayload(
			EventLoggerFileInfo next_fileinfo, // may be null if payload does not extend beyond single record 
			int nrec) throws IOException {
		EventLoggerFileInfo fileinfo = this;
		fileinfo.open();
		int [] full_type = fileinfo.getFullType(nrec); // 0x18
		if (full_type == null) {
			System.out.println("Wrong packet");
			return null; // error - length too large
		}
		
		int did_len = fileinfo.getIMX5Length(nrec);
		if (did_len < 0) {
			System.out.println("Wrong packet length (-"+did_len+")");
			return null; // error - length too large
		}
		byte [] payload = new byte [did_len];
		int bytes_left = did_len;
		int bytes_offset = 0;
		// read all/some from the initial sub packet;
		int subpacket = 0;
		while (bytes_left > 0) {
			int bytes_copied = fileinfo.copyPayload (
					nrec,         // int        nrec,
					payload,      // byte []    payload,
					bytes_offset, // int        offset,
					bytes_left);  // int        len) {
			bytes_left -= bytes_copied;
			bytes_offset += bytes_copied;
			subpacket ++;
			if (bytes_left > 0) {
				// find next subpacket, open new file if needed
				if (subpacket > EventLoggerFileInfo.REC_SUBPACKET_IMX5) {
					System.out.println("Too many subpackets (>"+EventLoggerFileInfo.REC_SUBPACKET_IMX5+")");
					return null; // throw - illegal subpacket index 
				}
				nrec = fileinfo.getNextIndex( // assuming open
						nrec,                     // int       indx,
						full_type[0] + subpacket, //  int       type,
						0); // full_type[1]); //int       did) // subpacket has no dids
				if (nrec < 0) {
					System.out.println("Reached the end of the current log file, proceeding to the next one");
					if ((next_fileinfo == null) || (next_fileinfo == fileinfo) ) {
						System.out.println("No log files left");
						return null;
					}
					fileinfo = next_fileinfo;
					fileinfo.open();
					nrec = fileinfo.getFirstLastIndex( // earliest in a file
							full_type[0] + subpacket,
							full_type[1])[0];
				}
			}
		}
		return payload;
	}
	
	
	
	public static int getRecType(ByteBuffer bb, int nrec) { // high microseconds byte with nibbles swapped
		int usec_msb = bb.get(nrec * REC_SIZE + LOCAL_USEC_OFFS + 3) & 0xff;
		if (usec_msb == 0) { // Probably log_imu.c tries to read too early? Full 32-byte line of 0
			System.out.println("getRecType(): usec_msb==0, nrec="+
		String.format("%d (0x%x) byte=0x%x", nrec, nrec, 64*nrec));
		}
		return ((usec_msb >> 4) & 0xf) + ((usec_msb & 0xf) << 4); 
	}

	public double getLocalTS(int nrec) {
		return getLocalTS(bb, nrec);
	}
	
	public static double getLocalTS(ByteBuffer bb, int nrec) {
		int usec = bb.getInt(nrec * REC_SIZE + LOCAL_USEC_OFFS) & 0xfffff;
		int sec =  bb.getInt(nrec * REC_SIZE + LOCAL_SEC_OFFS);
		return sec +  0.000001 * usec; 
	}

	// only applies to image trigger (0-3 - local channel, 4 - external trigger) 
	public static int getMasterChannel(ByteBuffer bb, int nrec) { // high microseconds byte with nibbles swapped
		int usec_msb = bb.get(nrec * REC_SIZE + MASTER_USEC_OFFS + 3) & 0xff;
//		return (usec_msb >> 4) + ((usec_msb & 0xff) << 4); 
		return usec_msb; 
	}
	
	public static double getMasterTSImg(ByteBuffer bb, int nrec) { // high microseconds byte with nibbles swapped
		int usec = bb.getInt(nrec * REC_SIZE + MASTER_USEC_OFFS) & 0xfffff;
		int sec =  bb.getInt(nrec * REC_SIZE + MASTER_SEC_OFFS);
		return sec + 0.000001 * usec; 
	}

	double getMasterTS(double ts_local) {
		return ts_master_start + (ts_local - ts_local_start) / local_rel_lag ;
	}

	double getMasterTS(ByteBuffer bb, int nrec) {
		return getMasterTS(getLocalTS(bb, nrec));
	}

	double getMasterTS(int nrec) {
		return getMasterTS(getLocalTS(bb, nrec));
	}

	/**
	 * Interpolate assuming linear difference between GPS/absolute and master times
	 * @param ts_master master timestamp (from the image name corrected to time zone and lag)
	 * @return time of week in seconds (not normalized, may be negative or >= week)
	 */
	double getLinearTimeOfWeek(double ts_master) {
		if (gnss_start[0] == gnss_end[0]) {
			return 0.001* (gnss_start[1] +
					(gnss_end[1]-gnss_start[1])*(ts_master-ts_master_start)/(ts_master_end-ts_master_start));
		} else { // if switched weeks count from start week
			return 0.001* (gnss_start[1] +
					(gnss_end[0] + Did_ins.WEEK_SECONDS +  gnss_end[1]-gnss_start[1])*(ts_master-ts_master_start)/(ts_master_end-ts_master_start));
		}
	}

	/**
	 * Use 1pps calibration to improve accuracy of the linear time of week
	 * @param ts_master
	 * @return TOW correction (subtract from the linear interpolation)
	 */
	double getTimeOfWeekCorrection(double ts_master) { // bb should be open
		int type_1pps = EventLoggerFileInfo.REC_TYPE_ODO << 4; // 0x30
		double [] first_last_ts = getFirstLastTS(type_1pps, 0);
		int [] first_last_index = getFirstLastIndex(type_1pps, 0); // should not be null as it matches getFirstLastTS()
		if (first_last_ts == null) {
			System.out.println ("ERROR: getCalibratedTimeOfWeek() getFirstLastTS(type_1pps, 0)-> null");
			return Double.NaN;
		}
		int index_1pps = -1;
		if (ts_master <= first_last_ts[0]) {
			index_1pps = first_last_index[0];
		} else if (ts_master >= first_last_ts[1]) {
			index_1pps = first_last_index[1];
		} else {
			index_1pps =  getLastBeforeIndex( // bb should be open
					ts_master, // double    ts_master,
					false,     // boolean   after, // false - before (including ==)
					type_1pps, // int       type,
					0); // int       did)
			if (index_1pps <0) {
				index_1pps =  getLastBeforeIndex( // bb should be open
						ts_master, // double    ts_master,
						true,     // boolean   after, // false - before (including ==)
						type_1pps, // int       type,
						0); // int       did)
			}
		}
		if (index_1pps <0) {
			System.out.println ("ERROR: getCalibratedTimeOfWeek() getFirstLastTS(type_1pps, 0)-> null");
			return Double.NaN;
		}
		double ts_1pps = getMasterTS(bb, index_1pps); // this record corresponds to zero fractional part of tow
		// pps_tow - interpolated should be close to integer
		double pps_tow = 0.001* (gnss_start[1] +
				(gnss_end[1]-gnss_start[1])*(ts_1pps-ts_master_start)/(ts_master_end-ts_master_start));
		double tow_corr = pps_tow-Math.round(pps_tow);
		// if tow_corr > 0, then interpolated value is too high, reduce it
		/*
		double linear_tow = 0.001* (gnss_start[1] +
				(gnss_end[1]-gnss_start[1])*(ts_master-ts_master_start)/(ts_master_end-ts_master_start));
		return linear_tow - tow_corr;
		*/
		return tow_corr;
	}

	
	// only applies to the first subpacket (rec_type = 0x18 - after swapping) of IMX5 packet
	public static int getIMX5Did(ByteBuffer bb, int nrec) {
		return bb.getShort(nrec * REC_SIZE + IMX5_DID) & 0xffff;
	}
	public int getIMX5Did(int nrec) {
		return getIMX5Did(bb, nrec);
	}
	
	public static int getIMX5Length(ByteBuffer bb, int nrec) {
		int len = bb.getShort(nrec * REC_SIZE + IMX5_LENGTH) & 0x7fff;
		int max_len = (REC_SIZE - IMX5_PAYLOAD_NEXT) + (REC_SIZE - IMX5_PAYLOAD_FIRST) * REC_SUBPACKET_IMX5;
		if (len > max_len) {
			return - len; // error
		}
		return len;
	}

	public int getIMX5Length(int nrec) {
		return getIMX5Length(bb, nrec);
	}
	
	public int copyPayload (
			int        nrec,
			byte []    payload,
			int        offset,
			int        len) {
		return copyPayload (
				bb, // ByteBuffer bb,
				nrec, // int        nrec,
				payload, // byte []    payload,
				offset, // int        offset,
				len); // int        len)
	}
	
	public static int copyPayload (
			ByteBuffer bb,
			int        nrec,
			byte []    payload,
			int        offset,
			int        len) {
		int [] full_type = getFullType(bb, nrec);
		if (full_type == null) {
			return 0;
		}
		boolean is_first = (full_type[0] & REC_SUBPACKET_IMX5) == 0;
		int payload_offset = is_first?IMX5_PAYLOAD_FIRST:IMX5_PAYLOAD_NEXT;
		int copy_len = Math.min(len, REC_SIZE - payload_offset);
		bb.position(nrec * REC_SIZE + payload_offset); // start of payload data
		bb.get(payload, offset, copy_len);
		System.arraycopy(bb.array(), nrec * REC_SIZE + payload_offset, payload, offset, copy_len);
		return copy_len;
	}
	
	
	public int [] getFullType(int nrec) {
		return getFullType(bb, nrec);
	}

	/**
	 * 
	 * Adding invalid image (usec == 0xffffff - what does it mean)?
	 * Check FPGA if it is the previous TS, not the current 
	 * @param bb
	 * @param nrec
	 * @return
	 */
	public static int [] getFullType(ByteBuffer bb, int nrec) {
		int [] types = new int[2];
		types[0] = getRecType(bb, nrec); // nibbles swapped
		int root_type = (types[0] >> 4) & 0xf; 
		switch (root_type) {
		case REC_TYPE_IMG: 
			types[1] = getMasterChannel(bb, nrec);
			if ((bb.getInt(nrec * REC_SIZE + MASTER_USEC_OFFS) & 0xfffff) == 0xfffff) {
				System.out.println("Wrong master TS (=0xfffff) for nrec=" + nrec);
				return null;
			}
			break;
		case REC_TYPE_GPS:
			int gps_subtype = types[0] & 0xf;
			if (gps_subtype == REC_SUBTYPE_IMX5) { // only first subpacket
				types[1] = getIMX5Did(bb, nrec);
			}
			break;
		case REC_TYPE_ODO:
			types[1] = 0; // not needed, just a placeholder
			break;
		}
		return types;	
	}
	@Override
	public int compareTo(EventLoggerFileInfo arg0) {
		if ((gnss_start != null) && (gnss_start[0] > 0) && (arg0.gnss_start != null) && (arg0.gnss_start[0] > 0)) {
			int c = Integer.compare(gnss_start[0],arg0.gnss_start[0]);
			if (c == 0) {
				c = Integer.compare(gnss_start[1],arg0.gnss_start[1]);
			}
			return c;
		}
// compare by GNSS time if available for both, by timestamps if not available		
		return Double.compare(ts_master_start,arg0.ts_master_start) ;
	}
	
}
