IMS interpolation

src/main/java/com/elphel/imagej/ims/Did_ins.java

@@ -14,14 +14,14 @@ public abstract class Did_ins <T extends Did_ins <T>>{
 	public double []		lla =   new double [3];
 	
 	static int interpolateInt(double frac, int v_this, int v_next) {
-		return (int) Math.round(frac * v_this + (1.0 - frac) * v_next);
+		return (int) Math.round(frac * v_next + (1.0 - frac) * v_this);
 	}
 	static float interpolateFloat(double frac, float v_this, float v_next) {
-		return (float) (frac * v_this + (1.0 - frac) * v_next);
+		return (float) (frac * v_next + (1.0 - frac) * v_this);
 	}
 
 	static double interpolateDouble(double frac, double v_this, double v_next) {
-		return frac * v_this + (1.0 - frac) * v_next;
+		return frac * v_next + (1.0 - frac) * v_this;
 	}
 	
 //https://www.swtestacademy.com/return-subclass-instance-java-generics/	

src/main/java/com/elphel/imagej/ims/EventLogger.java

@@ -3,6 +3,8 @@ 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;
@@ -33,39 +35,66 @@ public class EventLogger {
 		}
 		Arrays.sort(logger_files); // increasing start time stamps
 		
+		testInterpolateDidIns1();
+		testInterpolateDidIns1();
+		testInterpolateDidIns1();
+		
 //		String out_did_gps_path = "/home/elphel/lwir16-proc/office_04/did_gps";
 		String out_did_gps_path = dir+"../did_gps/did_gps";
+		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();
 		int gps_mask = 7; // +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
 		for (int nf = 0; nf < logger_files.length; nf++) {
-			System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path);
+			String out_path= out_did_gps_path+"-"+String.format("%03d", nf)+".out";
+			System.out.println("Printing all DID_GPS data in "+logger_files[nf].abs_path+" to "+out_path);
 			try {
-				logger_files[nf].listGPS(out_did_gps_path+"-"+String.format("%03d", nf)+".out", gps_mask, false);
+				logger_files[nf].listGPS(out_path, gps_mask, false);
 			} catch (IOException e) {
 				// TODO Auto-generated catch block
 				e.printStackTrace();
 			}
 		}
 		
-		
-		
-		
-		String out_did1_path = "/media/elphel/SSD3-4GB/lwir16-proc/berdich/ims/test_out/did_ins1";
+//		String out_did1_path = "/media/elphel/SSD3-4GB/lwir16-proc/berdich/ims/test_out/did_ins1";
+		String out_did1_path = dir+"../did_ins1/did_ins1";
+		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++) {
-			System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path);
+			String out_path= out_did1_path+"-"+String.format("%03d", nf)+".out";
+			System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
 			try {
-				logger_files[nf].listDid1(out_did1_path+"-"+String.format("%03d", nf)+".out",false);
+				logger_files[nf].listDid1(out_path,false);
 			} catch (IOException e) {
 				// TODO Auto-generated catch block
 				e.printStackTrace();
 			}
 		}
-		
-		
 		System.out.println("Processed "+logger_files.length+" event log files");
 	}
-	public void testInterpolate() {
+	
+	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
 	Did_ins_1 interpolateDidIns1(
 			double ts_master,

src/main/java/com/elphel/imagej/ims/EventLoggerFileInfo.java

@@ -120,13 +120,29 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		// 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) {
+		// 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)) {
+					break search_opposite; // return nrec;
+				}
+			}
+			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)) {
 					return nrec;
 				}
 			}
+		}
 		return first_last_index[after ? 1 : 0]; 
 	}
 	
@@ -211,7 +227,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 					if ((full_type[0] >> 4) == REC_TYPE_IMG) {
 						double ts_local =  getLocalTS(bb, nrec);
 						double ts_master = getMasterTSImg(bb, nrec);
-						if (debugLevel > -1) { // -1
+						if (debugLevel > 0) { // -1
 							System.out.println (
 									String.format("%6d %6d %10.6f",nimg, nrec, ts_master));
 						}
@@ -357,7 +373,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 						sx2 += ts_local * ts_local;
 						sy +=  ts_frac;
 						sxy += ts_local * ts_frac;
-						if (debugLevel > -1) {
+						if (debugLevel > 0) {
 							System.out.println (
 									String.format("%6d %10.6f %10.6f", nrec, ts_local, ts_frac));
 						}
@@ -410,9 +426,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
 		} 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("listDid1(): 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(