package imseProc.proc.demod;

import imseProc.core.ComplexImage;
import jafama.FastMath;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.LinkedList;

import otherSupport.StatusOutput;

import binaryMatrixFile.BinaryMatrixFile;

/** Automatic finding of (0,0), (+,+), (+,-) and (+,0) components in an FFT 
 * Works reasonably on clean FFTs, but not always.
 * 
 * @author oliford
 */
public class ComponentsFinder {
	//some shorthand 
	private static final int PP = 0; 
	private static final int P0 = 1; 
	private static final int PM = 2; 
	private static final int X = 0; 
	private static final int Y = 1; 
	
	public static final int[][] autoInitSelections(ComplexImage imageIn) {
		int iw = imageIn.getWidth(), ih = imageIn.getHeight();
		
		class Peak implements Comparable<Peak>{ 
			public Peak(int x, int y, double val) {
				this.x = x; this.y = y; this.val = val;
			}
			int x,y;
			double val;
			@Override
			public int compareTo(Peak o) { return -Double.compare(val, o.val); }			
		}
		LinkedList<Peak> peakList = new LinkedList<Peak>();
		
		int avgWidth = 2*(iw+ih) / 50;
		
		double maxPeak = 0;
		double smoothed[][] = new double[iw][ih];
		StatusOutput stat = new StatusOutput("DSHDemod auto select", (ih-avgWidth)*(iw-avgWidth));
		for(int y=avgWidth; y < (ih-avgWidth); y++){			
			for(int x=avgWidth; x < (iw-avgWidth); x++){
				for(int jX=-avgWidth; jX < avgWidth; jX++){
					for(int jY=-avgWidth; jY < avgWidth; jY++){
						double Re = imageIn.getReal(x+jX, y+jY);					
						double Im = imageIn.getImag(x+jX, y+jY);
						double abs = Re*Re + Im*Im;
						smoothed[x][y] += abs*abs*FastMath.exp(-(jX*jX + jY*jY)*9/(avgWidth*avgWidth));
					}
				}
				stat.doStatus(y*(iw-avgWidth) + x);
			}
		}
		stat.done();
		
		
		for(int y=avgWidth+1; y < (ih-avgWidth-1); y++){
			for(int x=avgWidth+1; x < (iw-avgWidth-1); x++){
				if(smoothed[x][y] > smoothed[x-1][y-1] &&
						smoothed[x][y] > smoothed[x-1][y] &&
						smoothed[x][y] > smoothed[x-1][y+1] &&
						smoothed[x][y] > smoothed[x][y-1] &&
						smoothed[x][y] > smoothed[x][y+1] &&
						smoothed[x][y] > smoothed[x+1][y-1] &&
						smoothed[x][y] > smoothed[x+1][y] &&
						smoothed[x][y] > smoothed[x+1][y+1]){
					peakList.add(new Peak(x, y, smoothed[x][y]));
				}
			}
		}
		BinaryMatrixFile.mustWrite("/tmp/a", smoothed, false);
		Collections.sort(peakList);
		
		ArrayList <Peak> topPeaks = new ArrayList<Peak>();
		for(int i=0; i < 20; i++){
			topPeaks.add(peakList.removeFirst());
		}
		
		System.out.println("Top 20:");
		for(Peak p : topPeaks){
			System.out.println("\t" + p.x + "\t"+ p.y + "\t" + p.val);
		}
		
		ArrayList<Peak> peakPairs = new ArrayList<Peak>();
		ArrayList<Peak> rejects = new ArrayList<Peak>();
		int j=0;
		outer: do{
			Peak p1 = topPeaks.remove(0);
			int p1FX = p1.x - iw/2;
			int p1FY = p1.y - ih/2;
			
			for(Peak p2 : topPeaks){
				int p2FX = p2.x - iw/2;
				int p2FY = p2.y - ih/2;
				
				if(FastMath.abs(p1FX + p2FX) < avgWidth && FastMath.abs(p1FY + p2FY) < avgWidth){
					topPeaks.remove(p2);
					System.out.println(p1FX + "\t" + p1FY + "\t" + p2FX + "\t" + p2FY);
					peakPairs.add(new Peak(p1FX, p1FY, (p1.val + p2.val) / 2));
					continue outer;
				}		
			}
			
			rejects.add(p1);
			
		}while(!topPeaks.isEmpty());
		
		System.out.println("Paired:");
		for(Peak p : peakPairs){
			System.out.println("\t" + p.x + "\t"+ p.y + "\t" + p.val);
		}

		System.out.println("Rejects");
		for(Peak p : rejects){
			System.out.println("\t" + p.x + "\t"+ p.y + "\t" + p.val);
		}
		
		Collections.sort(peakPairs);
		
		//we now look for which components sit directly inbetween two other components
		// these will be the (+,0) (0,+) (-,0) etc and the ones being averaged will be ++ +- etc
		// remembering that each has a pair		
		
		// Each of these is 2 corners and a centre
		ArrayList<Peak[]> tripples = new ArrayList<Peak[]>();
		for(int i1=0; i1 < peakPairs.size(); i1++){
			Peak p1 = peakPairs.get(i1);
			
			for(int i2=(i1+1); i2 < peakPairs.size(); i2++){
				Peak p2 = peakPairs.get(i2);
				
				for(Peak p3 : peakPairs){
					if(p3 == p1 || p3 == p2)
						continue;
					
					/// candidiate coords [set][++,+-,+0][x/y]
					int c[][][] = new int[][][]{
							{ { +p1.x, +p1.y }, { +p3.x, +p3.y }, { +p2.x, +p2.y } },
							{ { +p1.x, +p1.y }, { -p3.x, -p3.y }, { +p2.x, +p2.y } },
							{ { +p1.x, +p1.y }, { +p3.x, +p3.y }, { -p2.x, -p2.y } },
							{ { +p1.x, +p1.y }, { -p3.x, -p3.y }, { -p2.x, -p2.y } },
							{ { -p1.x, -p1.y }, { +p3.x, +p3.y }, { +p2.x, +p2.y } },
							{ { -p1.x, -p1.y }, { -p3.x, -p3.y }, { +p2.x, +p2.y } },
							{ { -p1.x, -p1.y }, { +p3.x, +p3.y }, { -p2.x, -p2.y } },
							{ { -p1.x, -p1.y }, { -p3.x, -p3.y }, { -p2.x, -p2.y } }
					};
					
					for(int k=0; k < 8; k++){							
						if( FastMath.abs(c[k][P0][X] - (c[k][PP][X] + c[k][PM][X])/2) < avgWidth && 
								FastMath.abs(c[k][P0][Y] - (c[k][PP][Y] + c[k][PM][Y])/2) < avgWidth) {
							tripples.add(new Peak[]{
									new Peak(c[k][PP][X], c[k][PP][Y], p1.val),
									new Peak(c[k][P0][X], c[k][P0][Y], p3.val),
									new Peak(c[k][PM][X], c[k][PM][Y], p2.val)});
						}
					}
					
					
				}
						
				
			}	
		}
		
		Collections.sort(tripples, new Comparator<Peak[]>() {
			@Override
			public int compare(Peak[] o1, Peak[] o2) {
				double v1=0, v2=0;
				for(int i=0; i < 3; i++){
					v1 += o1[i].val;
					v2 += o2[i].val;
				}	
				return -Double.compare(v1, v2);
			}
		});
		
		System.out.println("Tripples:");
		for(Peak p[] : tripples){
			System.out.println(p[0].x + "\t" + p[0].y + "\t" + 
								p[1].x + "\t" + p[1].y + "\t" + 
								p[2].x + "\t" + p[2].y);
		}
		
		Peak best[] = tripples.get(0);
		int d = (int)(FastMath.sqrt(FastMath.pow2((double)best[PM].x - best[PP].x) 
								+ FastMath.pow2((double)best[PM].y - best[PP].y))/5.0 + 0.5);
		int selections[][] = new int[DSHDemod.nComps][6];
		
		selections[DSHDemod.COMPONENT_00][0] = iw/2 -d; //x 
		selections[DSHDemod.COMPONENT_00][1] = ih/2 -d; //y
		selections[DSHDemod.COMPONENT_00][2] = 2*d; //w 
		selections[DSHDemod.COMPONENT_00][3] = 2*d; //h
		selections[DSHDemod.COMPONENT_00][4] = iw/2; //x0 
		selections[DSHDemod.COMPONENT_00][5] = ih/2; //y0

		selections[DSHDemod.COMPONENT_pp][0] = iw/2 + best[PP].x - d; //x 
		selections[DSHDemod.COMPONENT_pp][1] = ih/2 + best[PP].y - d; //y
		selections[DSHDemod.COMPONENT_pp][2] = 2*d; //w 
		selections[DSHDemod.COMPONENT_pp][3] = 2*d; //h
		selections[DSHDemod.COMPONENT_pp][4] = iw/2 + best[PP].x; //x0 
		selections[DSHDemod.COMPONENT_pp][5] = ih/2 + best[PP].y; //y0

		selections[DSHDemod.COMPONENT_p0][0] = iw/2 + best[P0].x - d; //x 
		selections[DSHDemod.COMPONENT_p0][1] = ih/2 + best[P0].y - d; //y
		selections[DSHDemod.COMPONENT_p0][2] = 2*d; //w 
		selections[DSHDemod.COMPONENT_p0][3] = 2*d; //h
		selections[DSHDemod.COMPONENT_p0][4] = iw/2 + best[P0].x; //x0 
		selections[DSHDemod.COMPONENT_p0][5] = ih/2 + best[P0].y; //y0

		selections[DSHDemod.COMPONENT_pm][0] = iw/2 + best[PM].x - d; //x 
		selections[DSHDemod.COMPONENT_pm][1] = ih/2 + best[PM].y - d; //y
		selections[DSHDemod.COMPONENT_pm][2] = 2*d; //w 
		selections[DSHDemod.COMPONENT_pm][3] = 2*d; //h
		selections[DSHDemod.COMPONENT_pm][4] = iw/2 + best[PM].x; //x0 
		selections[DSHDemod.COMPONENT_pm][5] = ih/2 + best[PM].y; //y0

		return selections;
		
	}
}