/**
 **
 ** UasLogRecord.java - Record structure for a single UAS telemetry log entry
 **
 ** Copyright (C) 2025-2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  UasLogRecord.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.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;

public class UasLogRecord {
	
	public static String [] FIELDS_USED = {
			"timestamp",
			"distanceFromHome",
			"gps_lat",
            "gps_lon",
            "gps_altitude",
            "homeLatitude",
            "homeLongitude"};
	
	public double timestamp; // should be first in array
	public double distance;
	public double gps_lat;
	public double gps_long;
	public double gps_alt; // relative to home
	public double home_lat;
	public double home_long;
	
	public UasLogRecord(
			double [] data) { 
		setData(data);
		
	}
	
	public void setData (
			double [] data) throws IllegalArgumentException{ 
		int n = 0;
		this.timestamp = data[n++];
		this.distance  = data[n++];
		this.gps_lat   = data[n++];
		this.gps_long  = data[n++];
		this.gps_alt   = data[n++]; // relative to home
		this.home_lat  = data[n++];
		this.home_long = data[n++];	
		if (Double.isNaN(this.timestamp)) {
			throw new IllegalArgumentException ("UasLogRecord).timestamp has to be defined");
		}
	}

	
	
	public void copyOverNaN(double [] data) {
		double [] arr = toArray();
		copyOverNaN(arr, data);
		for (int i = 0; i < arr.length; i++) {
			if (Double.isNaN(arr[i])) {
				arr[i] = data[i];
			}
		}
		setData(arr);
	}

	public static void copyOverNaN(double [] dst, double [] src) {
		for (int i = 0; i < dst.length; i++) {
			if (Double.isNaN(dst[i])) {
				dst[i] = src[i];
			}
		}
	}
	
	
	
	public double [] toArray() {
		double [] val = new double[FIELDS_USED.length];
		int n = 0;
		val[n++] = this.timestamp;
		val[n++] = this.distance;
		val[n++] = this.gps_lat;
		val[n++] = this.gps_long;
		val[n++] = this.gps_alt;
		val[n++] = this.home_lat;
		val[n++] = this.home_long;
		return val;		
	}
	
	public UasLogRecord(
			double timestamp,
			double distance,
			double gps_lat,
			double gps_long,
			double gps_alt, // relative to home
			double home_lat,
			double home_long) {
		this.timestamp = timestamp;
		this.distance  = distance;
		this.gps_lat   = gps_lat;
		this.gps_long  = gps_long;
		this.gps_alt   = gps_alt; // relative to home
		this.home_lat  = home_lat;
		this.home_long = home_long;		
	}
	
	public boolean hasUndefined() {
		return hasUndefined(toArray());
	}
	public static boolean hasUndefined(double [] arr) {
		for (double v: arr) if (Double.isNaN(v)) return true;
		return false;
		
	}
	
	public static UasLogRecord[] getArray(
			ArrayList<UasLogRecord> rec_list) {
		UasLogRecord [] rec_array = rec_list.toArray(new UasLogRecord[0]);
		Arrays.sort(rec_array, new Comparator<UasLogRecord>() { // decreasing weight
			@Override
			public int compare(UasLogRecord lhs, UasLogRecord rhs) {
				return (rhs.timestamp > lhs.timestamp) ? -1 : (rhs.timestamp < lhs.timestamp) ? 1 : 0;
			}
		});
		return rec_array;
		
	}
	
	public String toString() {
		return String.format("timestamp=%f, distance=%f, gps_lat=%f, gps_lon=%f, alt=%f, home_lat=%f, home_lon=%f",
				timestamp, distance, gps_lat, gps_long, gps_alt, home_lat, home_long);
	}
	
	public UasLogRecord clone() {
		return new UasLogRecord (toArray());
	}
	
	public static UasLogRecord interpolate(
			UasLogRecord[] rec_arr,
			double timestamp) {
		int last = rec_arr.length-1;
		double ts0 = rec_arr[0].timestamp;
		double ts1 = rec_arr[last].timestamp;
		if (timestamp <= ts0) {
			return rec_arr[0];
		} else if ( timestamp >= ts1) {
			return rec_arr[last];
		} else {
			// assuming timestamps are uniform, but not requiring that
			int indx = (int) Math.round(last * (timestamp-ts0)/(ts1-ts0));
			if (indx < 0) {
				indx = 0;
			} else if (indx > (last - 1)) {
				indx = last - 1;
			}
			while ((indx < (last-1)) && (rec_arr[indx+1].timestamp < timestamp)) indx++; // next record timestamp > requested
			while ((indx >=0) && (rec_arr[indx].timestamp > timestamp)) indx--;          // this record timestamp <= requested
			double k = (timestamp-rec_arr[indx].timestamp) / (rec_arr[indx+1].timestamp-rec_arr[indx].timestamp);
			double [] arr0 = rec_arr[indx].toArray();
			double [] arr1 = rec_arr[indx+1].toArray();
			double [] arr = new double [arr0.length];
			for (int i = 0; i < arr.length; i++) {
				arr[i] = arr0[i] + k * (arr1[i] - arr0[i]); 
			}
			return new UasLogRecord(arr);
		}
	}
	
	
	
	public static boolean fillUndefined(UasLogRecord[] rec_arr) {
//		UasLogRecord rec0=rec_arr[0].clone();
		// fill initial NaNs to the first record
		if (rec_arr[0].hasUndefined()) {
			double [] arr0 = rec_arr[0].toArray();
			fill_0: {
				for (int i = 0; i < rec_arr.length; i++) {
					if (!hasUndefined(arr0)) {
						break fill_0;
					}
					double [] v1= rec_arr[i].toArray();
					copyOverNaN(arr0, v1);
				}
				System.out.println("Still first undefined: "+(new UasLogRecord(arr0).toString()));
				return false;
			}
			rec_arr[0].setData(arr0);
		}
		// fill last
		int last = rec_arr.length-1;
		if (rec_arr[last].hasUndefined()) {
			double [] arr_last = rec_arr[last].toArray();
			fill_0: {
				for (int i = last; i >= 0; i--) {
					if (!hasUndefined(arr_last)) {
						break fill_0;
					}
					double [] v1= rec_arr[i].toArray();
					copyOverNaN(arr_last, v1);
				}
				System.out.println("Still last undefined: "+(new UasLogRecord(arr_last).toString()));
				return false;
			}
			rec_arr[last].setData(arr_last);
		}
		
		// interpolate assuming timestamp is always known
		// first and last element should never be NaN here
		double [][] all_arr = new double [rec_arr.length][];
		for (int i = 0; i < all_arr.length; i++) {
			all_arr[i] = rec_arr[i].toArray();
		}
		int num_fields = all_arr[0].length;
		for (int nfield = 1; nfield < num_fields; nfield++) { // 0 is the timestamp
			for (int tail = 0; tail <= last; tail++) {
				for (; (tail < last) && !Double.isNaN(all_arr[tail][nfield]); tail++);
				if (tail < last) {
					tail--;
					// tail is the last non-NaN here
					int head = tail+2;
					for (; (head <= last) && Double.isNaN(all_arr[head][nfield]); head++);
					// head is the first non-NaN after NaN
					double time_span =   all_arr[head][0] - all_arr[tail][0];
					double value_span =  all_arr[head][nfield] - all_arr[tail][nfield];
					double tail_val = all_arr[tail][nfield];
					for (int i = tail+1; i < head; i++) {
						double since_tail = all_arr[i][0] - all_arr[tail][0];
						all_arr[i][nfield] = tail_val + value_span*since_tail/time_span;
					}
					tail = head; // will be added 1
				}
			}
		}
		for (int i = 0; i < all_arr.length; i++) {
			rec_arr[i].setData(all_arr[i]);
		}		
		return true;
	}
	
	
	
	
}
