package com.elphel.imagej.ims;

import java.awt.Point;
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.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;

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;
	// 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]));
		int drec = after ? 1: -1;
		for (int nrec = nrec0; (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)) {
				return nrec;
			}
		}
		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 (String abs_path, boolean keep) 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;
		}
		rec_first_last = new int [rec_types.length][];
		{ // processing local to master timestamps correspondence
			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 > -1) { // -1
							System.out.println (
									String.format("%6d %6d %10.6f",nimg, nrec, ts_master));
						}
						nimg++;
						if (Double.isNaN(ts_local0)) {
							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;
					}
				}
			}
			// calculate offsets and timestamp range (master?)
			double d = (s0 * sx2 - sx * sx);
			double a = (sxy * s0 - sy * sx) / d;
			double b = (sx2 * sy - sx * sxy) / d;
			ts_local_start =        getLocalTS(bb, 0);
			double ts_local_end =   getLocalTS(bb, num_recs - 1);
			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;  
		}
		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 ;
			}
		}
		// Timing calibration with DID_STROBE_IN_TIME
		// 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) {
			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) {
					System.out.println(String.format("gnss_start: week=%d, sec=%f", gnss_start[0], 0.001*gnss_start[1]));
					System.out.println(String.format("gnss_end:   week=%d, sec=%f", gnss_end[0],   0.001*gnss_end[1]));
				}
			}
		}
		
		// 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.
		
		int type_1pps = REC_TYPE_ODO << 4;
		int indx_1pps = getTypeIndex (type_1pps, 0);
		if (indx_1pps >= 0) {
			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 > -1) {
							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));
				}
			}			
		}
		
		
		if (!keep) {
			close();
		}
	}
	
	public void listDid1(String path, boolean keep) throws IOException {
		double max_offset = 1000; // m
		open();
		float [] start_ned = null;
		FileWriter writer = null;
		if (path != null) {
			writer = new FileWriter (path);
		}
		String header = "record "+
		        "     master TS    "+
		        "     local TS     "+
		        "timeOfWeek " +
		        " insStat "+
		        " hdwStat "+
		        "               theta               "+
		        "           uvw                "+
		        "                 lla                      "+
		        "              ned                \n"; 
		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) && (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 %17.6f %17.6f %10.3f %08x %08x [%8.4f, %8.4f, %8.4f] "+
				        "[%8.3f, %8.3f, %8.3f] [%12.7f, %12.7f, %12.7f] [%8.3f, %8.3f, %8.3f]\n",
						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
						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 (writer != null) {
					writer.write(line);
				} else {
					System.out.print(line);
				}
			}
		}
		if (writer != null) {
			writer.close();
		}
		if (!keep) {
			close();
		}
	}

	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 "+
		        "     master TS    "+
		        "     local TS     "+
		        "timeOfWeek " +
		        "did "+
		        " status  "+
		        "                    ecef                     "+
		        "                    lla                     "+
		        "  hMSL  "+ // 7.2f
		        "  hAcc  "+ // 7.3f
		        "  vAcc  "+ // 7.3f
		        "  pDop  "+ // 7.3f
		        "cnoMean "+ // 7.3f
		        "  towOffset "+ // 11.4f
		        " lS  " + // 3d
		        "sat " + // 3d
		        "sgm " + // 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;
				}
				
				// replace with GPS fix type?
				/*
				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 %17.6f %17.6f %10.3f %3d %08x [%13.4f, %13.4f, %13.4f] "+
				        "[%12.7f, %12.7f, %12.7f] %7.2f %7.3f %7.3f %7.3f %7.3f %11.4f "+
						"%3d %3d %3d %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();
		}
	}
	
	
	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));
	}

	
	
	// 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) {
		return Double.compare(ts_master_start,arg0.ts_master_start) ;
	}
	
}
