EventLogger.java

package com.elphel.imagej.ims;

import java.io.File;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.stream.Collectors;
import java.util.stream.Stream;

import ij.Prefs;

public class EventLogger {
	public  EventLoggerFileInfo [] logger_files;
	public EventLogger (String dir, String debug_prefix) { // dir should contain only event logger files
		if (!dir.endsWith(Prefs.getFileSeparator())) {
			dir+=Prefs.getFileSeparator();
		}
		ArrayList<String> files = new ArrayList<String>( // Set
				Stream.of(new File(dir).listFiles())
			      .filter(file -> !file.isDirectory())
			      .map(File::getName)
			      .collect(Collectors.toSet()));
		logger_files = new EventLoggerFileInfo [files.size()];
		for (int nf = 0; nf < logger_files.length; nf++) { // may use threads (per file)
			String debug_path= (debug_prefix != null) ? (debug_prefix+nf+".csv") : null;
			try {
				logger_files[nf] = new EventLoggerFileInfo(dir+files.get(nf), false, debug_path);
			} catch (IOException e) {
				logger_files[nf] = null;
				e.printStackTrace();
			}
		}
		Arrays.sort(logger_files); // increasing start time stamps
		
//		testInterpolateDidIns1();
//		testInterpolateDidIns1();
//		testInterpolateDidIns1();
		System.out.println("Processed "+logger_files.length+" event log files");
	}
	
	public void printGps(
			String out_did_gps_path,
			int gps_mask) { // 7: // +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
		out_did_gps_path = Paths.get(out_did_gps_path).normalize().toString();
		Path op_did_gps = Paths.get(out_did_gps_path);
		op_did_gps=op_did_gps.normalize();
		(new File(op_did_gps.getParent().toString())).mkdirs();
		for (int nf = 0; nf < logger_files.length; nf++) {
			String out_path= out_did_gps_path+"-"+String.format("%03d", nf)+".csv";
			System.out.println("Printing all DID_GPS data in "+logger_files[nf].abs_path+" to "+out_path);
			try {
				logger_files[nf].listGPS(out_path, gps_mask, false);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		
	}
	
	public void printDidIns1(String out_did1_path) {
		out_did1_path = Paths.get(out_did1_path).normalize().toString();
		Path op_did1 = Paths.get(out_did1_path);
		op_did1=op_did1.normalize();
		(new File(op_did1.getParent().toString())).mkdirs();
		for (int nf = 0; nf < logger_files.length; nf++) {
			String out_path= out_did1_path+"-"+String.format("%03d", nf)+".csv";
			System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
			try {
				logger_files[nf].listDidIns1(out_path,false);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}

	public void printDidIns2(String out_did2_path) {
		out_did2_path = Paths.get(out_did2_path).normalize().toString();
		Path op_did2 = Paths.get(out_did2_path);
		op_did2=op_did2.normalize();
		(new File(op_did2.getParent().toString())).mkdirs();
		for (int nf = 0; nf < logger_files.length; nf++) {
			String out_path= out_did2_path+"-"+String.format("%03d", nf)+".csv";
			System.out.println("Printing all DID_INS_2 data in "+logger_files[nf].abs_path+" to "+out_path);
			try {
				logger_files[nf].listDidIns2(out_path,false);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
	
	public void printPimu(String out_pimu_path) {
		out_pimu_path = Paths.get(out_pimu_path).normalize().toString();
		Path op_pimu = Paths.get(out_pimu_path);
		op_pimu=op_pimu.normalize();
		(new File(op_pimu.getParent().toString())).mkdirs();
		for (int nf = 0; nf < logger_files.length; nf++) {
			String out_path= out_pimu_path+"-"+String.format("%03d", nf)+".csv";
			System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
			try {
				logger_files[nf].listPimu(out_path,false);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
	
	public void printStrobeInTime(String out_sit_path) {
		out_sit_path = Paths.get(out_sit_path).normalize().toString();
		Path op_sit = Paths.get(out_sit_path);
		op_sit=op_sit.normalize();
		(new File(op_sit.getParent().toString())).mkdirs();
		for (int nf = 0; nf < logger_files.length; nf++) {
			String out_path= out_sit_path+"-"+String.format("%03d", nf)+".csv";
			System.out.println("Printing all DID_STROBE_IN_TIME data in "+logger_files[nf].abs_path+" to "+out_path);
			try {
				logger_files[nf].listStrobeInTime(out_path,false);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}

	public void print2Pps(String out_1pps_path) {
		out_1pps_path = Paths.get(out_1pps_path).normalize().toString();
		Path op_1pps = Paths.get(out_1pps_path);
		op_1pps=op_1pps.normalize();
		(new File(op_1pps.getParent().toString())).mkdirs();
		for (int nf = 0; nf < logger_files.length; nf++) {
			String out_path= out_1pps_path+"-"+String.format("%03d", nf)+".csv";
			System.out.println("Printing all DID_STROBE_IN_TIME data in "+logger_files[nf].abs_path+" to "+out_path);
			try {
				logger_files[nf].list1Pps(out_path,false);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
	
	
	public void testInterpolateDidIns1() {
		boolean exit_now = false;
		double ts_master = 1694576078.939126;
		while (!exit_now) {
			Did_ins_1 d1=null;
			try {
				d1=interpolateDidIns1(
						ts_master,
						0);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			} // debug_level) throws IOException {
			System.out.println("ts_master="+ts_master);
		}
		
	}
	
	
	//Did_ins_1
	public Did_ins_1 interpolateDidIns1(
			double ts_master,
			int       debug_level) throws IOException {
		byte [][] payload = new byte[2][];
		int did = Imx5.DID_INS_1;
		double frac = getDidAtTS(
				ts_master, // double    ts_master,
				did, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
				payload, // byte [][] payload,
				debug_level); // int       debug_level)
		if (Double.isNaN(frac)) {
			if (debug_level > -1){
				System.out.println("interpolateDidIns1(): Failed to extract DID_INS_1 data");
			}
			return null;
		}
		Did_ins_1 did_below = new Did_ins_1(ByteBuffer.wrap(payload[0]));
		Did_ins_1 did_above = new Did_ins_1(ByteBuffer.wrap(payload[1]));
		return did_below.interpolate(
				frac, // double frac,
				did_above); // Did_ins_1 next_did))
	}
	//Did_ins_2
	public Did_ins_2 interpolateDidIns2(
			double ts_master,
			int       debug_level) throws IOException {
		byte [][] payload = new byte[2][];
		int did = Imx5.DID_INS_2;
		double frac = getDidAtTS(
				ts_master, // double    ts_master,
				did, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
				payload, // byte [][] payload,
				debug_level); // int       debug_level)
		if (Double.isNaN(frac)) {
			if (debug_level > -1){
				System.out.println("interpolateDidIns1(): Failed to extract DID_INS_2 data");
			}
			return null;
		}
		Did_ins_2 did_below = new Did_ins_2(ByteBuffer.wrap(payload[0]));
		Did_ins_2 did_above = new Did_ins_2(ByteBuffer.wrap(payload[1]));
		return did_below.interpolate(
				frac, // double frac,
				did_above); // Did_ins_1 next_did))
	}
	
	public Did_gps_pos interpolateDidGpsPos(
			double ts_master,
			int    did, // Imx5.DID_GPS1_POS, Imx5.DID_GPS2_POS, Imx5.DID_GPS1_UBX_POS
			int       debug_level) throws IOException {
		byte [][] payload = new byte[2][];
		double frac = getDidAtTS(
				ts_master, // double    ts_master,
				did, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
				payload, // byte [][] payload,
				debug_level); // int       debug_level)
		if (Double.isNaN(frac)) {
			if (debug_level > -1){
				System.out.println("interpolateDidIns1(): Failed to extract DID_GPS_POS data");
			}
			return null;
		}
		Did_gps_pos did_below = new Did_gps_pos(ByteBuffer.wrap(payload[0]));
		Did_gps_pos did_above = new Did_gps_pos(ByteBuffer.wrap(payload[1]));
		return did_below.interpolate(
				frac, // double frac,
				did_above); // Did_gps_pos next_did))
	}
	
	public Did_pimu interpolateDidPimu(
			double ts_master,
			int       debug_level) throws IOException {
		byte [][] payload = new byte[2][];
		int did = Imx5.DID_PIMU;
		double frac = getDidAtTS(
				ts_master, // double    ts_master,
				did, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
				payload, // byte [][] payload,
				debug_level); // int       debug_level)
		if (Double.isNaN(frac)) {
			if (debug_level > -1){
				System.out.println("interpolateDidIns1(): Failed to extract DID_PIMU data");
			}
			return null;
		}
		Did_pimu did_below = new Did_pimu(ByteBuffer.wrap(payload[0]));
		Did_pimu did_above = new Did_pimu(ByteBuffer.wrap(payload[1]));
		return did_below.interpolate(
				frac, // double frac,
				did_above); // Did_ins_1 next_did))
	}
	
	
	
	public double getDidAtTS(
			double    ts_master,
			int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
			byte [][] payload,
			int       debug_level) throws IOException // initialize to [2][] - will return a pair of byte arrays
	{
		int min_debug = 1; //-1 to enable
		// 1 find file with the first subpacket
		int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
		int file_index_below = 0; // if ts_master is between the two files? use last of previous
		search_file: {
			for (; file_index_below < logger_files.length; file_index_below++) {
				double [] first_last_ts = logger_files[file_index_below].getFirstLastTS(type, did); 
				if (first_last_ts != null ) {
					if ((first_last_ts[0] <= ts_master) && (first_last_ts[1] >= ts_master)) { 
						break search_file; // this file contains info
					}
					// as files are ordered in ascending timestamps, no sense to go beyond
					if (first_last_ts[0] > ts_master) {
						if (file_index_below == 0) {
							if (debug_level > -1){
								System.out.println("getDidAtTS(): Required timestamp is too early");
							}
							return Double.NaN;
						}
						file_index_below--; // use last in the previous file
						break;
					}
				}
			}
			if (debug_level > -1){
				System.out.println("getDidAtTS(): Required timestamp is too late");
			}
			return Double.NaN; // here? Use last of ...
		}
		logger_files[file_index_below].open();
		int nrec_below = logger_files[file_index_below].getLastBeforeIndex( // bb should be open
				ts_master, // double    ts_master,
				false,     // boolean   after, // false - before (including ==)
				type,      // int       type,
				did);      // int       did)
		if (nrec_below < 0) {
			System.out.println("getDidAtTS(): Something is wrong, could not get record for "+type+":"+did);
			return Double.NaN;
		}
		
		int file_index_above = file_index_below; //
		// first try in the same file
		int nrec_above = logger_files[file_index_above].getLastBeforeIndex( // bb should be open
				ts_master, // double    ts_master,
				true,     // boolean   after, // false - before (including ==)
				type,      // int       type,
				did);      // int       did)
		if (nrec_above < 0) {
			file_index_above++;
			if (file_index_above >= logger_files.length) {
				if (debug_level > -1){
					System.out.println("getDidAtTS(): No next record for interpolation available");
				}
				return Double.NaN; // no next file
			}
			logger_files[file_index_above].open();
			nrec_above = logger_files[file_index_above].getFirstLastIndex(type, did)[0]; // first of this type
		}
		double ts_below = logger_files[file_index_below].getMasterTS(nrec_below); 
		double ts_above = logger_files[file_index_above].getMasterTS(nrec_above);
		double frac = (ts_master - ts_below)/(ts_above - ts_below);
		if ((debug_level >= min_debug) && (did == Imx5.DID_INS_2)) {
			System.out.println (String.format("%6d - %6d frac=%6.4f, ts_master = %17.6f, ts_below = %17.6f, ts_above = %17.6f",
					nrec_below, nrec_above, frac, ts_master, ts_below, ts_above));
		}
		
		// now read both byte arrays and set byte [][] payload
		EventLoggerFileInfo fileinfo_next_below =
				(file_index_below < (logger_files.length -1))?  logger_files[file_index_below + 1] : null;
		EventLoggerFileInfo fileinfo_next_above =
				(file_index_above < (logger_files.length -1))?  logger_files[file_index_above + 1] : null;
		payload[0] = logger_files[file_index_below].getDidPayload(
				fileinfo_next_below, // , // int file_index,
				nrec_below);      // int nrec)
		payload[1] = logger_files[file_index_above].getDidPayload( // was file_index_below
				fileinfo_next_above, // , // int file_index,
				nrec_above);      // int nrec)
		if ((payload[0] == null) || (payload[1] == null)) {
			if (debug_level > -1){
				if (payload[0] == null) {
					System.out.println("getDidAtTS(): failed to get payload[0]");
				}
				if (payload[1] == null) {
					System.out.println("getDidAtTS(): failed to get payload[1]");
				}
			}
			return Double.NaN;
		}
		// if it is DID_INS_1 or DID_INS_2 use TOW for calculating fraction
		if ((did == Imx5.DID_INS_1) || (did == Imx5.DID_INS_2)) {
			double tow_corr = logger_files[file_index_below].getTimeOfWeekCorrection(ts_master);
			int week0, week1;
			double tow0, tow1;
			if (!Double.isNaN(tow_corr)) { 
				double tow = logger_files[file_index_below].getLinearTimeOfWeek(ts_master) - tow_corr;
				@SuppressWarnings("rawtypes")
				Did_ins did_ins_0 = (did == Imx5.DID_INS_2) ?
						(new Did_ins_2(ByteBuffer.wrap(payload[0]))) : (new Did_ins_1(ByteBuffer.wrap(payload[0])));
				@SuppressWarnings("rawtypes")
				Did_ins did_ins_1 = (did == Imx5.DID_INS_2) ?
						(new Did_ins_2(ByteBuffer.wrap(payload[1]))) : (new Did_ins_1(ByteBuffer.wrap(payload[1])));
				tow0 = did_ins_0.timeOfWeek;
				tow1 = did_ins_1.timeOfWeek;
				if (did_ins_1.week > did_ins_0.week) {
					tow1+=	Did_ins.WEEK_SECONDS;
				}
				frac = (tow - tow0) /(tow1-tow0);
			} else {
				System.out.println("ERROR: getDidAtTS(): tow_corr is NaN, keeping frac="+frac);
			}
		}
		return frac; 
	}
	
	/*
	
	byte [] getDidPayload(
			int file_index,
			int nrec) throws IOException {
		logger_files[file_index].open();
		int [] full_type = logger_files[file_index].getFullType(nrec); // 0x18
		int did_len = logger_files[file_index].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 = logger_files[file_index].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 = logger_files[file_index].getNextIndex( // assuming open
						nrec,                     // int       indx,
						full_type[0] + subpacket, //  int       type,
						full_type[1]); //int       did)
				if (nrec < 0) {
					System.out.println("Reached the end of the current log file, proceeding to the next one");
					file_index++;
					if (file_index >= logger_files.length) {
						System.out.println("No log files left");
						return null;
					}
					logger_files[file_index].open();
					nrec = logger_files[file_index].getFirstLastIndex( // earliest in a file
							full_type[0] + subpacket,
							full_type[1])[0];
				}
			}
		}
		return null;
	}
	*/
	
}