/**
 **
 ** UasLogReader.java - UAS flight telemetry log reader for multi-sensor data fusion
 **
 ** Copyright (C) 2025-2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  UasLogReader.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 org.json.JSONArray;
import org.json.JSONException;
import org.json.JSONObject;

import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.QuadCLT;

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
//import java.nio.charset.StandardCharsets;
import java.nio.file.Files;
//import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Calendar;
import java.util.HashMap;

public class UasLogReader {
	// just for testing
	public static double [] CAMERA_LLA = {40.36068739259896, -112.02054681495696, 1507.4378478095987};
//	public double [] uas_home_ned = null; // use if non-null; if null - rely on accurate GPS (camera GPS relative to the UAS GPS) 
	public double [] ned_corr = new double[3]; // add to calculated NED.  
	UasLogRecord[] rec_arr;
	double         start_timestamp;
	double         uas_home_alt = -1.2; // relative to the camera, table + mount height
	double []      camera_lla = CAMERA_LLA.clone();
	double []      camera_ATR = new double [] {-2.78, 0.1252, 0}; //  double[3];
	QuadCLT        parentCLT = null; // for geometic parameters

	
	public void setUASHomeNed(double [] uas_home_ned){
		// get NED for the UAS home
		if (uas_home_ned == null) {
			ned_corr = new double[] {0,0,0}; // -uas_home_alt};
		} else {
			ned_corr = uas_home_ned.clone();
			/*
			double [] camera_lla0 = {camera_lla[0], camera_lla[1], 0};
			UasLogRecord uas_home = this.rec_arr[0];
			double [] uas_home_lla = {uas_home.home_lat, uas_home.home_long, 0};
			double [] ned = Imx5.nedFromLla (
					uas_home_lla,      //  double [] lla,
					camera_lla0); // double [] lla_ref)
			ned_corr = new double [3];
			for (int i = 0; i < 3; i++) {
				ned_corr[i] = 	uas_home_ned[i] - ned[i];
			}
			*/
		}
		return;
	}
	
	public void setCameraLLA(double [] camera_lla){
		if (camera_lla != null) {
			this.camera_lla = camera_lla.clone();
			System.out.println("UasLogReader.setCameraLLA(): lat="+camera_lla[0]+" lon="+camera_lla[1]+" alt="+camera_lla[2]+
					" (reference altitude used for NED: uas_home_alt="+uas_home_alt+" -> camera_lla0_alt="+(-uas_home_alt)+")");
		}
	}

	public void setCameraATR(double [] atr) {
		if (atr != null) {
			this.camera_ATR = atr.clone();
			System.out.println("UasLogReader.setCameraATR(): az="+atr[0]+" tilt="+atr[1]+" roll="+atr[2]+
					" (az_deg="+(atr[0]*180.0/Math.PI)+")");
		}
	}
	
	public double [] getCameraATR() {
		return this.camera_ATR;
	}

	public double [] getCameraLLA() {
		return this.camera_lla.clone();
	}

	public double getUasHomeAlt() {
		return this.uas_home_alt;
	}

	public double [] getNedCorr() {
		return this.ned_corr.clone();
	}
	
	public double getStartTimestamp() {
		return start_timestamp;
	}
	
	public void setStartTimestamp(double timestamp) {
		this.start_timestamp = timestamp;
	}
	
	public UasLogReader(
			String filePath,
			double timestamp, 
			double [] camera_lla,
			QuadCLT   parentCLT) throws JSONException {
		this.parentCLT = parentCLT;
		setCameraLLA(camera_lla);
		if (DjiSrtReader.looksLikeSrtPath(filePath)) {
			loadDjiSrt(filePath, timestamp);
		} else {
			setStartTimestamp(timestamp);
			loadJson(filePath);
		}
		if ((rec_arr == null) || (rec_arr.length == 0)) {
			throw new IllegalArgumentException("No UAS log records loaded from " + filePath);
		}
	}

	private void loadDjiSrt(
			String filePath,
			double timestamp) {
		DjiSrtReader djiSrtReader = new DjiSrtReader(filePath);
		rec_arr = djiSrtReader.getRecords();
		double startTs = timestamp;
		if (startTs == 0.0) {
			startTs = djiSrtReader.getFirstEpochSeconds();
			System.out.println(String.format(
					"UasLogReader: using DJI SRT first timestamp %.3f as cuasUasTimeStamp (input was 0)",
					startTs));
		}
		setStartTimestamp(startTs);
		System.out.println(String.format(
				"UasLogReader: loaded DJI SRT path \"%s\": %d records, startTimestamp=%f, source files=%d",
				filePath,
				rec_arr.length,
				start_timestamp,
				djiSrtReader.getSourceFiles().length));
	}

	private void loadJson(
			String filePath) throws JSONException {
		JSONObject logData = null;
		JSONArray flight_logging_items = null;
		JSONArray flight_logging_keys = null;
		HashMap<String, Integer> log_indices = null;
		try {
			String jsonString = new String(Files.readAllBytes(Paths.get(filePath)));
			logData = new JSONObject(jsonString);
			flight_logging_items = logData.getJSONObject("exchange").getJSONObject("message").getJSONObject("flight_logging").getJSONArray("flight_logging_items");
			flight_logging_keys =  logData.getJSONObject("exchange").getJSONObject("message").getJSONObject("flight_logging").getJSONArray("flight_logging_keys");
			log_indices = new HashMap<String, Integer>();

		} catch (IOException e) {
			System.err.println("Error reading file: " + e.getMessage());
		}
		if (log_indices == null) {
			throw new IllegalArgumentException("Failed to parse UAS JSON log: " + filePath);
		}

		for (int i = 0; i < flight_logging_keys.length(); i++) {
			log_indices.put(flight_logging_keys.getString(i), i);
		}
		int [] indices =  new int[UasLogRecord.FIELDS_USED.length];
		for (int i = 0; i < UasLogRecord.FIELDS_USED.length; i++) {
			Integer idx = log_indices.get(UasLogRecord.FIELDS_USED[i]);
			if (idx == null) {
				throw new IllegalArgumentException(
						"Missing field \"" + UasLogRecord.FIELDS_USED[i] + "\" in UAS JSON log " + filePath);
			}
			indices[i] = idx;	
		}
		ArrayList<UasLogRecord> uas_log_list = new ArrayList<UasLogRecord>();
		double[] rec = new double [indices.length];
		for (int indx = 0; indx < flight_logging_items.length(); indx++) {
			JSONArray log_line =flight_logging_items.getJSONArray(indx);
			for (int i = 0; i < UasLogRecord.FIELDS_USED.length; i++) {
				Object obj = log_line.get(indices[i]);
				if ((obj instanceof Integer) && (((Integer) obj) == -1)) {
					rec[i] = Double.NaN;
				} else {
					rec[i] = Double.parseDouble(log_line.get(indices[i]).toString());
				}
			}
			try {
				uas_log_list.add(new UasLogRecord(rec));
			} catch (IllegalArgumentException e) {
				System.err.println("Skipping undefined timestamp");
			}
		}            
		rec_arr = UasLogRecord.getArray(
				uas_log_list); // ArrayList<UasLogRecord> rec_list)            

		UasLogRecord.fillUndefined(rec_arr);
		System.out.println("UasLogReader: loaded JSON log " + filePath + ", records=" + rec_arr.length);
	}

	public UasLogRecord interpolate(double timestamp) { // relative
		return UasLogRecord.interpolate(
				this.rec_arr,
				timestamp-start_timestamp); 
	}

	public double [] getNED(double timestamp) {
		UasLogRecord uas_loc = interpolate(timestamp);
		double [] camera_lla0 = {camera_lla[0], camera_lla[1], -uas_home_alt};
		double [] uas_lla = {uas_loc.gps_lat, uas_loc.gps_long, uas_loc.gps_alt};
		double [] ned = Imx5.nedFromLla (
				uas_lla,      //  double [] lla,
				camera_lla0); // double [] lla_ref)
		for (int i = 0; i < 3; i++) {
			ned[i] += ned_corr[i];
		}
		return ned;
	}

	/** Returns {lat, lon, alt, north, east, down} for the given scene timestamp string. */
	public double [] getUasLlaNed(String stimestamp) {
		double timestamp = Double.parseDouble(stimestamp.replace("_", "."));
		UasLogRecord uas_loc = interpolate(timestamp);
		double [] camera_lla0 = {camera_lla[0], camera_lla[1], -uas_home_alt};
		double [] uas_lla = {uas_loc.gps_lat, uas_loc.gps_long, uas_loc.gps_alt};
		double [] ned = Imx5.nedFromLla(uas_lla, camera_lla0);
		for (int i = 0; i < 3; i++) ned[i] += ned_corr[i];
		return new double[] {uas_lla[0], uas_lla[1], uas_lla[2], ned[0], ned[1], ned[2]};
	}
	
/*
>>> NED = [-1757.6742631032464, -581.1933833090495, -60.759999999999785]          
>>> az=math.atan2(NED[1],NED[0])
>>> az
-2.822249609529323
>>> 180/math.pi*az
-161.70299135847475

 */
	
	public static double degToRad(double a) {
		return a/180.0*Math.PI;
	}
	public static double radToDeg(double a) {
		return a*180.0/Math.PI;
	}
	
	public void testOrient(double [] camera_atr, double ts, QuadCLT parentCLT) {
		setCameraATR(camera_atr);
		double [] ned = getNED(ts);
		
		
		System.out.println(String.format("north = %f, east = %f, down = %f @ time = %f s", ned[0], ned[1], ned[2],ts));
		double [] wxyz = {-ned[1], -ned[2], -ned[0]}; // right - positive X, up -positive Y, away - negative Z
		double uas_az = Math.atan2(wxyz[0], -wxyz[2]);
		double uas_tl = Math.atan2(wxyz[1], -wxyz[2]);
				
		System.out.println(String.format("uas world az = %fdeg, tl = %fdeg)", radToDeg(uas_az),radToDeg(uas_tl)));
		

		System.out.println(String.format("world x = %f, y = %f, z = %f)", wxyz[0], wxyz[1], wxyz[2]));
		
		
		double [] atr = getCameraATR();
		double [] iatr = ErsCorrection.invertXYZATR(
				new double[3], //double [] source_xyz,
				atr)[1]; // double [] source_atr)[1];

		double [] camera_xyz0 = ErsCorrection.applyXYZATR( 
				new double [3], //  double [] reference_xyz,
				atr,  // double [] reference_atr,
				wxyz); // double [] scene_xyz)
		/*
		double [] camera_xyz1 = ErsCorrection.applyXYZATR( 
				wxyz, //  double [] reference_xyz,
				atr, // double [] reference_atr,
				new double [3]); // double [] scene_xyz)
		
		double [] camera_xyz2 = ErsCorrection.applyXYZATR( 
				new double [3], //  double [] reference_xyz,
				iatr,  // double [] reference_atr,
				wxyz); // double [] scene_xyz)

		double [] camera_xyz3 = ErsCorrection.applyXYZATR( 
				wxyz, //  double [] reference_xyz,
				iatr, // double [] reference_atr,
				new double [3]); // double [] scene_xyz)
*/
		System.out.println(String.format("camera0 x = %f, y = %f, z = %f)", camera_xyz0[0], camera_xyz0[1], camera_xyz0[2]));
		/*
		System.out.println(String.format("camera1 x = %f, y = %f, z = %f)", camera_xyz1[0], camera_xyz1[1], camera_xyz1[2]));
		System.out.println(String.format("camera2 x = %f, y = %f, z = %f)", camera_xyz2[0], camera_xyz2[1], camera_xyz2[2]));
		System.out.println(String.format("camera3 x = %f, y = %f, z = %f)", camera_xyz3[0], camera_xyz3[1], camera_xyz3[2]));
		*/
		double [] wxyz4 = {wxyz[0],wxyz[1],wxyz[2], 1.0}; // 0.0 for infinity
		boolean correctDistortions = true;
		ErsCorrection ec = parentCLT.getErsCorrection();
		double [] pXpYD = ec.getImageCoordinatesERS(
				wxyz4, // double [] xyzw,
				correctDistortions, // boolean correctDistortions, // correct distortion (will need corrected background too !)
				new double[3], // double [] camera_xyz, // camera center in world coordinates
				atr); // double [] camera_atr); // camera orientation relative to world frame

		System.out.println(String.format("pXpYD= [%f, %f,%f]", pXpYD[0], pXpYD[1], pXpYD[2]));
		return;
	}

	public double [] getUasPxPyD (
			String   stimestamp) {
		double timestamp = Double.parseDouble(stimestamp.replace("_", "."));
		return getUasPxPyD(timestamp);
	}
	
	public double [] getUasPxPyDRange (
			String   stimestamp) {
		double [] uasPxPyD = getUasPxPyD (stimestamp);
		if (uasPxPyD == null) {
			return null;
		}
		double range = getUasRange(stimestamp);
		return new double[] {uasPxPyD[0],uasPxPyD[1],uasPxPyD[2], range};
	}	
	
	
	public double [] getUasPxPyD (
			double   timestamp) {
		if (parentCLT == null) {
			return null;
		}
		double [] ned = getNED(timestamp);
		double [] wxyz4 = { ned[1], -ned[2], -ned[0], 1.0}; // 0.0 for infinity
		double [] atr = getCameraATR();
		boolean correctDistortions = true;
		ErsCorrection ec = parentCLT.getErsCorrection();
		double [] pXpYD = ec.getImageCoordinatesERS(
				wxyz4, // double [] xyzw,
				correctDistortions, // boolean correctDistortions, // correct distortion (will need corrected background too !)
				new double[3], // double [] camera_xyz, // camera center in world coordinates
				atr); // double [] camera_atr); // camera orientation relative to world frame
		return pXpYD;
	}

	public double getUasRange(double   timestamp) {
		double [] ned = getNED(timestamp);
		return Math.sqrt(ned[0]*ned[0]+ned[1]*ned[1]+ned[2]*ned[2]);
	}
	public double getUasRange(String stimestamp) {
		double timestamp = Double.parseDouble(stimestamp.replace("_", "."));
		return getUasRange(timestamp);
	}
	
	//

	
	public static void testUasLogReader(
			String filePath,
			QuadCLT           parentCLT
			) throws JSONException {
		// just for now
		UasLogReader uasLogReader = new UasLogReader(filePath, 0, null, parentCLT);
		double [] camera_atr = new double [] {-2.78, 0.1252, 0}; // {degToRad(-161),degToRad(16),0};
		double ts = 135.0;
		
		uasLogReader.testOrient(camera_atr,ts, parentCLT);
		
		
		
		/*
    		String [] field_to_print = {
    				"timestamp",
    				"distanceFromHome",
    				"gps_lat",
                    "gps_lon",
                    "gps_altitude",
                    "homeLatitude",
                    "homeLongitude"};
		 */
		StringBuffer sb = new StringBuffer();
		sb.append(new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(Calendar.getInstance().getTime())+"\n");
		try {
			String jsonString = new String(Files.readAllBytes(Paths.get(filePath)));
			JSONObject logData = new JSONObject(jsonString);
			/*
                        647.8000000000103,
                        40.36068344116211,
                        -112.02062225341797,

			 * 
                String fancy_data = logData.toString(4); 
                try {
                    Path path = Paths.get(filePath+".txt");
                    Files.write(path, fancy_data.getBytes(StandardCharsets.UTF_8));
                    System.out.println("String successfully written to file: " + path);
                } catch (IOException e) {
                    System.err.println("An error occurred while writing to the file: " + e.getMessage());
                }
			 */
			JSONArray flight_logging_items = logData.getJSONObject("exchange").getJSONObject("message").getJSONObject("flight_logging").getJSONArray("flight_logging_items");
			JSONArray flight_logging_keys =  logData.getJSONObject("exchange").getJSONObject("message").getJSONObject("flight_logging").getJSONArray("flight_logging_keys");
			//                String [] log_keys = new String [flight_logging_keys.length()];

			HashMap<String, Integer> log_indices = new HashMap<String, Integer>();
			for (int i = 0; i < flight_logging_keys.length(); i++) {
				log_indices.put(flight_logging_keys.getString(i), i);
			}
			int [] indices =  new int[UasLogRecord.FIELDS_USED.length];
			for (int i = 0; i < UasLogRecord.FIELDS_USED.length; i++) {
				indices[i] = log_indices.get(UasLogRecord.FIELDS_USED[i]);	
			}
			sb.append("index\t");
			for (int i = 0; i < UasLogRecord.FIELDS_USED.length; i++) {
				sb.append(UasLogRecord.FIELDS_USED[i]);
				sb.append((i < (UasLogRecord.FIELDS_USED.length - 1))?"\t":"\n");
			}
			for (int indx = 0; indx < flight_logging_items.length(); indx++) {
				sb.append(indx+"\t");
				JSONArray log_line =flight_logging_items.getJSONArray(indx);
				for (int i = 0; i < UasLogRecord.FIELDS_USED.length; i++) {
					sb.append(log_line.get(indices[i]));
					sb.append((i < (UasLogRecord.FIELDS_USED.length - 1))?"\t":"\n");
				}            	
			}
		} catch (IOException e) {
			System.err.println("Error reading file: " + e.getMessage());
		}
		try {
			BufferedWriter out = new BufferedWriter(
					new FileWriter(filePath+".csv", true));
			out.write(sb.toString());
			out.close();
		} catch (IOException e) {
			// Display message when exception occurs
			System.out.println("exception occurred" + e);
			return; //  false;
		}

		return;




		/*
            "flight_logging_keys": [
                "timestamp",
                "gps_lat",
                "gps_lon",
                "gps_altitude",
                "speed_vx",
                "speed_vy",
                "zSpeed",
                "dronePitch",
                "droneRoll",
                "droneYaw",
                "mLeftHorizontal",
                "mLeftVertical",
                "mRightHorizontal",
                "mRightVertical",
                "batteryCapacityPercentage",
                "phoneHeading",
                "currentJourney",
                "distanceFromHome",
                "battery_voltage",
                "currentElectricity",
                "currentCurrent",
                "visionWarning",
                "visionExtWarning",
                "visionErrorCode",
                "radarInfoTimeStamp",
                "frontRadarInfo",
                "rearRadarInfo",
                "leftRadarInfo",
                "rightRadarInfo",
                "topRadarInfo",
                "bottomRadarInfo",
                "alertArry",
                "remoteLatitude",
                "remoteLongitude",
                "flightMode",
                "gimbalPitch",
                "gimbalRoll",
                "gimbalYaw",
                "gpsSignalLevel",
                "rcRSSI",
                "rcModeState",
                "rcPowerState",
                "rcTakeOffState",
                "rcHomeState",
                "rcHoverState",
                "cameraMode",
                "mMode",
                "timeLeft",
                "homeLatitude",
                "homeLongitude",
                "satelliteCount",
                "designedVolume",
                "fullChargeVolume",
                "batteryTemperature",
                "remainPowerPercent",
                "numberOfDischarge",
                "cellCount",
                "maxFlightAltitude",
                "voltageOfCells",
                "goHomeAltitude",
                "beginnerModeEnable",
                "lowBatteryWarningThreshold",
                "seriousBatteryWarningThreshold",
                "maxFlightRadius",
                "maxFlightHorizontalSpeed",
                "obstacleAvoidanceEnable",
                "radarEnable",
                "gearMode",
                "backTime",
                "imageName",
                "imagePath"
            ],

		 */
	}




}
