/**
 **
 ** EventLogger.java - Multi-sensor event logger for synchronized UAS data recording
 **
 ** Copyright (C) 2023-2025 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  EventLogger.java is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ** -----------------------------------------------------------------------------**
 **
 */
package com.elphel.imagej.ims;

import java.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 com.elphel.imagej.cameras.CLTParameters;

import ij.Prefs;

public class EventLogger {
	public  EventLoggerFileInfo [] logger_files;
	private static EventLogger eventLogger = null;
//	public static synchronized boolean hasEventLogger() { // 
//		return eventLogger != null;
//	}
	public static synchronized EventLogger getEventLogger() {
		return eventLogger;
	}
	
	public static synchronized EventLogger getEventLogger(
			CLTParameters clt_parameters,
			String dir,
			String debug_prefix) {
		if (eventLogger == null) {
			eventLogger = new EventLogger (
					clt_parameters,  // CLTParameters clt_parameters,
					dir,
					debug_prefix);
			System.out.println("Created eventLogger");
		}
		return eventLogger;
	}
	private EventLogger (
			CLTParameters clt_parameters,			
			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(
						clt_parameters, // CLTParameters clt_parameters,
						dir+files.get(nf), // String abs_path,
						false,             // boolean keep,
						debug_path);       // debugLevelInner); // int debugLevel);
			} catch (IOException e) {
				logger_files[nf] = null;
				e.printStackTrace();
			}
		}
		Arrays.sort(logger_files); // increasing start time stamps
		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, double [] vert_xyz) {
		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";
			if (logger_files[nf].hasIns(false)) {
				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, vert_xyz);
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			} else {
				System.out.println("DID_INS_1 data is not available in "+logger_files[nf].abs_path+" to "+out_path);

			}
		}
	}

	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";
			if (logger_files[nf].hasIns(true)) {
				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();
				}
			} else {
				System.out.println("DID_INS_2 data is not available in "+logger_files[nf].abs_path+" to "+out_path);

			}
		}
	}
	
	public double [][] getQuats(
			double ts_start,
			double ts_end){
		if (ts_start < 0) ts_start=Double.NaN;
		if (ts_end   < 0) ts_end=Double.NaN;
		double [][] quats_all = null;
		double [][] quats =    null;
		for (int nf = 0; nf < logger_files.length; nf++) {
			quats = null;
			if (logger_files[nf].hasIns(true)) {
				System.out.println("Processing all DID_INS_2 data in "+logger_files[nf].abs_path);
				try {
					quats = logger_files[nf].getQuats(
							ts_start, // double ts_start, // or NaN
							ts_end,   // double ts_end, // or NaN
							false);   // boolean keep) throws IOException {

				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			} else {
				System.out.println("DID_INS_2 data is not available in "+logger_files[nf].abs_path);
			}
			if ((quats == null) || (quats.length ==0)) {
				System.out.println("Processing all DID_INS_1 data in "+logger_files[nf].abs_path);
				try {
					quats = logger_files[nf].getQuatsIns1(
							ts_start, // double ts_start, // or NaN
							ts_end,   // double ts_end, // or NaN
							false);   // boolean keep) throws IOException {

				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			if ((quats != null) && (quats.length > 0)) {
				if (quats_all == null) {
					quats_all = quats;
				} else {
					double [][] quats0 = quats_all;
					quats_all = new double[quats0.length + quats.length][];
					System.arraycopy(quats0, 0, quats_all, 0,             quats0.length);
					System.arraycopy(quats,  0, quats_all, quats0.length, quats.length);
				}
			}
		}		
		return quats_all;
	}
	
	
	
	
	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_PIMU 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)
		Did_ins_2 did_below = null;
		Did_ins_2 did_above = null;

		if (!Double.isNaN(frac)) {
			did_below = new Did_ins_2(ByteBuffer.wrap(payload[0]));
			did_above = new Did_ins_2(ByteBuffer.wrap(payload[1]));
		} else {
			// try to recover from did_ins_1;
			float [][] quats = new float [2][];
			frac =  getDidAtTS(
					ts_master,      // double    ts_master,
					Imx5.DID_INS_1, // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
					payload,        // byte [][] payload,
					quats,          // float [][]  quats, // recovering from missing did_ins_2. Null - regular way
					debug_level);   // int       debug_level)
			if (Double.isNaN(frac)) {
				if (debug_level > -1){
					System.out.println("interpolateDidIns21(): Failed to extract DID_INS_2 or DID_INS_1 data");
				}
				return null;
			}
			did_below = new Did_ins_2(ByteBuffer.wrap(payload[0]), quats[0]); // get Did_ins_1 first, then add quaternion data
			did_above = new Did_ins_2(ByteBuffer.wrap(payload[1]), quats[1]); // get Did_ins_1 first, then add quaternion data
			
		}
//		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
	{
		return getDidAtTS(
				ts_master,    // double    ts_master,
				did,          // int       did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
				payload,      // byte [][] payload,
				null,         // float []  quats, // recovering from missing did_ins_2. Null - regular way
				debug_level); // int       debug_level);
	}
	
	public double getDidAtTS(
			double      ts_master,
			int         did,   // DID value (4 for DID_INS_1, 3 for DID_PIMU) 
			byte [][]   payload,
			float [][]  quats, // recovering from missing did_ins_2. Null - regular way
			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 or missing DID.");
			}
			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;
		}
		if (quats != null) {
			logger_files[file_index_below].setInsQuats(); // generate quaternions (if they do not exist)
			quats[0] = logger_files[file_index_below].getQuat(nrec_below);
		}
		
		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
		}
		if (quats != null) {
			logger_files[file_index_above].setInsQuats(); // generate quaternions (if they do not exist)
			quats[1] = logger_files[file_index_above].getQuat(nrec_above);
		}
		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; 
	}
	
	
}
