package com.elphel.imagej.tileprocessor;

import java.io.File;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.util.Arrays;
import java.util.Calendar;
import java.util.HashMap;
import java.util.HashSet;
import java.util.concurrent.atomic.AtomicInteger;

import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;

import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.ims.Imx5;

public class SeriesInfinityCorrection {
	public static final String SUFFIX_SERIES_INFINITY = "-series_infiniti.csv"; 
	


	
	
	public static double getInfinityMuliSeries(
    		CLTParameters    clt_parameters,
    		QuadCLT []       refCLTs,
    		QuadCLT          index_CLT, // normally - just the last in quadCLTs
    		double [][]      infinity_rslt,
    		final int        debugLevel) {
		boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
		for (QuadCLT ref_scene:refCLTs) {
			ref_scene.restoreAnyDSI(debugLevel);
		}
		// get all scenes used in at least one segment.
		HashSet<String> scene_set = new HashSet<String>();
		HashMap<String,QuadCLT> ref_map = new HashMap<String,QuadCLT>(); 
		for (QuadCLT ref_scene:refCLTs) {
			ref_map.put(ref_scene.getImageName(), ref_scene);
			ErsCorrection ers_reference = ref_scene.getErsCorrection();
			String [] fl_ts = ref_scene.getFirstLastTimestamps();
			boolean good_fl = (fl_ts != null) && (fl_ts[0] != null) && (fl_ts[1] !=null);
			String [] names =good_fl ? ers_reference.getScenes(fl_ts[0],fl_ts[1]) : ers_reference.getScenes(); // others, referenced by reference
			for (String s:names)scene_set.add(s);
		}
		String [] all_scene_names = scene_set.toArray(new String[0]);
		Arrays.sort(all_scene_names);
		HashMap<String,QuadCLT> scene_map = new HashMap<String,QuadCLT>(); 
		String models_dir = index_CLT.correctionsParameters.x3dDirectory;
		final String suffix = index_CLT.correctionsParameters.imsSuffix; // assuming common for all
		final QuadCLT[] scenes = new QuadCLT[all_scene_names.length];
		
		final Thread[] threads = ImageDtt.newThreadArray();
		final AtomicInteger ai = new AtomicInteger(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				public void run() {
					for (int iScene = ai.getAndIncrement(); iScene < scenes.length; iScene = ai.getAndIncrement()) {
						String scene_name = all_scene_names[iScene];
						QuadCLT rscene = ref_map.get(scene_name);
						// create new if did not exist,
						if (rscene == null) { 
							scenes[iScene] = new QuadCLT(index_CLT,scene_name);
						} else {
							scenes[iScene] = rscene;
						}
						Path ims_path = Paths.get(models_dir).resolve(scene_name).resolve(scene_name+suffix);
						scenes[iScene].restoreIms(
								clt_parameters, // // CLTParameters clt_parameters,
								ims_path.toString(), // String ims_path,
								true, // boolean create,
								debugLevel-1); // debugLevelInner); // int debugLevel);
					}
				}
			};
		}		      
		ImageDtt.startAndJoin(threads);
		for (int nscene = 0; nscene < all_scene_names.length; nscene++) { // scene_name:all_scene_names) {
			scene_map.put(all_scene_names[nscene], scenes[nscene]);
		}
		// Now iterate through the reference scenes, using IMS data from the scene_map scenes.
		// initially will use earliest and latest scenes only. Improve for the future non-linear flying multicopters
		// and account for sync loss (far samples with random coordinates)
		double [] img_scales =  new double[refCLTs.length];
		double [] dist_visual = new double[refCLTs.length];
		double [] dist_ims2 =   new double[refCLTs.length];
		double [] disp_corrs=   new double[refCLTs.length];
		for (int nref = 0; nref < refCLTs.length; nref++) {
			QuadCLT ref_scene = refCLTs[nref];
			ErsCorrection ers_reference = ref_scene.getErsCorrection();
			// This does not work, as there can be stale in ers_reference.getScenes();
//			String [] sts = ers_reference.getScenes();
//			String [] first_last_ts = {sts[0],sts[sts.length - 1]};
			String [] first_last_ts = ref_scene.getFirstLastTimestamps();
			
			double[][] xyz_fl = new double [first_last_ts.length][];
			Did_ins_2 [] did_2_fl= new Did_ins_2 [first_last_ts.length];
			for (int n = 0; n < first_last_ts.length; n++) {
				String ts= first_last_ts[n];
				xyz_fl[n] = ers_reference.getSceneXYZ(ts);
				did_2_fl[n] = scene_map.get(ts).did_ins_2;
			}
			double travel_visual2 = 0;
			double travel_ims2 = 0;
			double [] dned = Imx5.nedFromLla (did_2_fl[1].lla, did_2_fl[0].lla);
			for (int i = 0; i < 3; i++) {
				double dv = xyz_fl[1][i]-xyz_fl[0][i];
				travel_visual2 += dv*dv;
				travel_ims2 += dned[i]*dned[i];
			}
			dist_visual[nref] = Math.sqrt(travel_visual2);
			dist_ims2[nref] =   Math.sqrt(travel_ims2);
			double img_scale = Math.sqrt(travel_visual2/travel_ims2);
			img_scales[nref] =  img_scale;
			double disp_corr = 	getImsDisparityCorrection(
					img_scale,   // double           scale_img,
					ref_scene,   // QuadCLT          ref_CLT,
					use_lma_dsi, // boolean          use_lma_dsi,
					debugLevel); // final int        debugLevel) {
			disp_corrs[nref] = disp_corr;
		}
		double sw = 0.0, swd = 0.0; // , sw_vis=0, sw_ims=0;
		for (int nref = 0; nref < refCLTs.length; nref++) {
			double w = 1.0;
			sw += w;
			swd += w * disp_corrs[nref];
//			sw_vis += dist_visual[nref];
//			sw_ims += dist_ims2[nref];
		}
		double disp_corr = swd/sw;
		if (infinity_rslt != null) {
			for (int nref = 0; nref < refCLTs.length; nref++) {
				infinity_rslt[nref] = new double[] {disp_corrs[nref], img_scales[nref], dist_visual[nref], dist_ims2[nref]};
			}			
		}
		/*
		if (debugLevel > -3) {
			System.out.println("ts\timg_scale\tdist_visual\tdist_ims");
			for (int nref = 0; nref < refCLTs.length; nref++) {
				System.out.println(refCLTs[nref].getImageName()+"\t"+img_scales[nref]+"\t"+dist_visual[nref]+"\t"+dist_ims2[nref]);
			}			
			System.out.println("Average\t"+img_scale+"\t"+(sw_vis/sw)+"\t"+(sw_ims/sw));
		}
		*/
    	return disp_corr;
    }
	
	public static double getImsDisparityCorrection(
			double           scale_img,
			QuadCLT          ref_CLT,
			boolean          use_lma_dsi,
			final int        debugLevel) {
		boolean dbg=debugLevel > 1000;
		// ref_scene.restoreAnyDSI(debugLevel);
		double [][] dls = ref_CLT.getDLS();
		if (dls == null) {
			ref_CLT.restoreAnyDSI(debugLevel);
			dls = ref_CLT.getDLS();
			if (dls == null) {
				System.out.println("*** BUG: getImsDisparityCorrection(): dsi==null and no dsi file is available");
				return Double.NaN;
			}
		}
		double [] disparity_dsi = dls[use_lma_dsi?1:0]; // [1]; // null
		double [] strength = ref_CLT.getDLS()[2];
		double sw=0,swd=0; // old, new-old
		int num_good = 0;
		if (dbg) {
			ShowDoubleFloatArrays.showArrays(
					new double[][] {disparity_dsi, strength},
					ref_CLT.getTilesX(),
					ref_CLT.getTilesY(),
					true,
					ref_CLT.getImageName()+"getImsDisparityCorrection",
					new String[] {"disparity","strength"}); //	dsrbg_titles);
		}

		for (int i = 0; i < disparity_dsi.length; i++) {
			if (!Double.isNaN(disparity_dsi[i])) {
				if (Double.isNaN(strength[i])) {
					System.out.println("getImsDisparityCorrection(): strength["+i+"]=NaN");
				} else {
					sw   += strength[i];
					swd  += strength[i]*disparity_dsi[i];
					num_good++;
				}
			}
		}
		double disp_avg = swd/sw;
		if (debugLevel > -3) {
			System.out.println("getImsDisparityCorrection() sw ="+sw+" swd="+swd+" disp_avg="+disp_avg+
					" num_good="+num_good+" in reference scene "+ref_CLT.getImageName());
		}

		double inf_disp_ref = disp_avg * (1.0 - scale_img);
		if (debugLevel > -3) {
			System.out.println("Disparity at infinity ="+inf_disp_ref+" in reference scene "+ref_CLT.getImageName());
		}
		return inf_disp_ref;
	}

	
	public static void saveInfinitySeries(
//    		CLTParameters    clt_parameters,
    		QuadCLT []       refCLTs,
    		QuadCLT          index_CLT, // normally - just the last in quadCLTs
    		double [][]      infinity_rslt,
    		int              debugLevel) {
		String header = "ts\tinfinity\timg_scale\tdist_visual\tdist_ims\n";
		StringBuffer sb = new StringBuffer();
 		double sw = 0.0, swd = 0.0, sw_scale = 0, sw_vis=0, sw_ims=0;
		for (int nref = 0; nref < refCLTs.length; nref++) {
			double w = 1.0;
			sw += w;
			swd += w *   infinity_rslt[nref][0]; // disparity
			sw_scale +=  infinity_rslt[nref][1];
			sw_vis +=    infinity_rslt[nref][2];
			sw_ims +=    infinity_rslt[nref][3];
			sb.append(refCLTs[nref].getImageName()+"\t"+infinity_rslt[nref][0]+"\t"+infinity_rslt[nref][1]+"\t"+infinity_rslt[nref][2]+"\t"+infinity_rslt[nref][3]+"\n");
		}
		sb.append("Average\t"+(swd/sw)+"\t"+(sw_scale/sw)+"\t"+(sw_vis/sw)+"\t"+(sw_ims/sw)+"\n");
	    Path inf_path = Paths.get(index_CLT.getX3dDirectory(true)).resolve(index_CLT.getImageName()+SUFFIX_SERIES_INFINITY);
		CalibrationFileManagement.saveStringToFile (
				inf_path.toString(),
				header+sb.toString());
		if (debugLevel > -3) {
			System.out.println("Infinity correction data saved to "+inf_path.toString());
		}
		return;
	}


    
}
