/**
 * @return list of transformation results for each source location in this warp field's grid.
 *         For each grid point, local coordinates are the grid source locations and
 *         world coordinates are the corresponding transformed result.
 */
public List<Point> getGridPoints() {

    final List<Point> gridPoints = new ArrayList<>();

    final double pixelsPerRow = affineWarpField.getYScale();
    final double pixelsPerHalfRow = pixelsPerRow / 2.0;
    final double pixelsPerColumn = affineWarpField.getXScale();
    final double pixelsPerHalfColumn = pixelsPerColumn / 2.0;

    double x;
    double y;
    for (int row = 0; row < affineWarpField.getRowCount(); row+=1) {
        y = locationOffsets[1] + (row * pixelsPerRow) + pixelsPerHalfRow;
        for (int column = 0; column < affineWarpField.getColumnCount(); column+=1) {
            x = locationOffsets[0] + (column * pixelsPerColumn) + pixelsPerHalfColumn;
            final double[] local = new double[] {x, y};
            final double[] world = apply(local);
            gridPoints.add(new Point(local, world));
        }
    }

    return gridPoints;
}
 

/**
 * Fits sampled points to a model.
 *
 * Stolen from
 *
 * <a href="https://github.com/axtimwalde/fiji-scripts/blob/master/TrakEM2/visualize-ct-difference.bsh#L90-L106">
 *     https://github.com/axtimwalde/fiji-scripts/blob/master/TrakEM2/visualize-ct-difference.bsh#L90-L106
 * </a>.
 *
 * @param  model                model to fit (note: model will be changed by this operation).
 * @param  coordinateTransform  transform to apply to each sampled point.
 * @param  sampleWidth          width of each sample.
 * @param  sampleHeight         height of each sample.
 * @param  samplesPerDimension  number of samples to take in each dimension.
 */
public static void fit(final Model<?> model,
                       final CoordinateTransform coordinateTransform,
                       final double sampleWidth,
                       final double sampleHeight,
                       final int samplesPerDimension)
        throws NotEnoughDataPointsException, IllDefinedDataPointsException {

    final List<PointMatch> matches = new ArrayList<>();

    for (int y = 0; y < samplesPerDimension; ++y) {
        final double sampleY = y * sampleHeight;
        for (int x = 0; x < samplesPerDimension; ++x) {
            final double sampleX = x * sampleWidth;
            final Point p = new Point(new double[]{sampleX, sampleY});
            p.apply(coordinateTransform);
            matches.add(new PointMatch(p, p));
        }
    }

    model.fit(matches);
}
 

public static List<PointMatch> convertMatchesToLocal(final List<PointMatch> worldMatchList,
                                               final TileSpec pMatchTileSpec,
                                               final TileSpec qMatchTileSpec) {

    final List<PointMatch> localMatchList = new ArrayList<>(worldMatchList.size());
    Point pPoint;
    Point qPoint;
    for (final PointMatch worldMatch : worldMatchList) {
        try {
            pPoint = getLocalPoint(worldMatch.getP1(), pMatchTileSpec);
            qPoint = getLocalPoint(worldMatch.getP2(), qMatchTileSpec);
            localMatchList.add(new PointMatch(pPoint, qPoint));
        } catch (final NoninvertibleModelException e) {
            LOG.warn("skipping match", e);
        }
    }
    return localMatchList;
}
 

/**
 * Sample the average scaling of a given {@link CoordinateTransform} by transferring a set of point samples using
 * the {@link CoordinateTransform} and then least-squares fitting a {@link SimilarityModel2D} to it.
 *
 * @param width  of the samples set
 * @param height of the samples set
 * @param dx     spacing between samples
 *
 * @return average scale factor
 */
public static double sampleAverageScale(final CoordinateTransform ct,
                                        final int width,
                                        final int height,
                                        final double dx) {
    final ArrayList<PointMatch> samples = new ArrayList<>();
    for (double y = 0; y < height; y += dx) {
        for (double x = 0; x < width; x += dx) {
            final Point p = new Point(new double[]{x, y});
            p.apply(ct);
            samples.add(new PointMatch(p, p));
        }
    }
    final AffineModel2D model = new AffineModel2D();
    try {
        model.fit(samples);
    } catch (final NotEnoughDataPointsException | IllDefinedDataPointsException e) {
        LOG.warn("failed to fit samples, returning scale factor of 1", e);
        return 1;
    }
    final double[] data = new double[6];
    model.toArray(data);
    // return 1;
    return Math.sqrt(Math.max(data[0] * data[0] + data[1] * data[1], data[2] * data[2] + data[3] * data[3]));
}
 

protected void addSimplePoints( final ArrayList<Point> points1, final ArrayList<Point> points2 )
{
	points1.add( new Point( new double[]{ 0, 0, 0 } ) );
	points1.add( new Point( new double[]{ 0, 0, 1.1f } ) );
	points1.add( new Point( new double[]{ 0, 1.2f, 0 } ) );
	points1.add( new Point( new double[]{ 1.3f, 0, 0 } ) );
	points1.add( new Point( new double[]{ 1.3f, 1.4f, 0 } ) );

	final Point offset = new Point( new double[]{ 1, 2, 3 } );
	
	for ( final Iterator<Point> i = points1.iterator(); i.hasNext(); )
	{
		final Point p2 = new Point( i.next().getL().clone() );
		add( p2, offset );
		points2.add( p2 );
	}

	points1.add( new Point( new double[]{ 0.1f, 0.1f ,0.1f } ) );		
}
 

@Override
      public void paintOnTop(final Graphics2D g, final Display display, final Rectangle srcRect, final double magnification) {

	final Stroke original_stroke = g.getStroke();
	final AffineTransform original = g.getTransform();
	g.setTransform( new AffineTransform() );
	g.setStroke( new BasicStroke( 1.0f ) );
	for ( final Point p : points )
	{
		final double[] w = p.getW();
		Utils.drawPoint( g, ( int )Math.round( magnification * ( w[ 0 ] - srcRect.x ) ), ( int )Math.round( magnification * ( w[ 1 ] - srcRect.y ) ) );
	}

	g.setTransform( original );
	g.setStroke( original_stroke );
}
 

/** Returns the index of the closest point, with accuracy depending on magnification. */
synchronized public int find(final double x_p, final double y_p, final double mag) {
	int index = -1;
	double d = 10 / mag;
	if (d < 2) d = 2;
	double min_dist = Integer.MAX_VALUE;
	int i = 0;
	final Point ref = new Point(new double[]{x_p, y_p});
	for (final Point p : points) {
		final double dist = Point.distance(ref, p);
		if (dist <= d && dist <= min_dist) {
			min_dist = dist;
			index = i;
		}
		i++;
	}
	return index;
}
 

synchronized public void paint(final Graphics2D g, final Rectangle srcRect, final double mag) {
	g.setColor(Color.yellow);
	g.setFont(new Font("SansSerif", Font.BOLD, 14));
	int i = 1;
	for (final Point p : points) {
		final double[] w = p.getW();
		final int x = (int)((w[0] - srcRect.x) * mag);
		final int y = (int)((w[1] - srcRect.y) * mag);
		// draw a cross at the exact point
		g.setColor(Color.black);
		g.drawLine(x-4, y+1, x+4, y+1);
		g.drawLine(x+1, y-4, x+1, y+4);
		g.setColor(Color.yellow);
		g.drawLine(x-4, y, x+4, y);
		g.drawLine(x, y-4, x, y+4);
		// draw the index
		g.drawString(Integer.toString(i), x+5, y+5);
		i++;
	}
}
 

public BlockMatchResults(final Collection<? extends Point> v1,
                         final Collection<? extends Point> v2,
                         final Collection<PointMatch> pm12,
                         final Collection<PointMatch> pm21,
                         final boolean layer1Fixed,
                         final boolean layer2Fixed,
                         final Triple<Integer, Integer, AbstractModel<?>> pair)
{
    this.v1 = v1;
    this.v2 = v2;
    this.pm12 = pm12;
    this.pm21 = pm21;
    this.layer1Fixed = layer1Fixed;
    this.layer2Fixed = layer2Fixed;
    this.pair = pair;
}
 

public BlockMatchPairCallable(final Triple<Integer, Integer, AbstractModel<?>> pair,
                              final List<Layer> layerRange,
                              final boolean layer1Fixed,
                              final boolean layer2Fixed,
                              final Filter<Patch> filter,
                              final ElasticLayerAlignment.Param param,
                              final Collection< ? extends Point > sourcePoints1,
                              final Collection< ? extends Point > sourcePoints2,
                              final Rectangle box)
{
    this.pair = pair;
    layer1 = layerRange.get(pair.a);
    layer2 = layerRange.get(pair.b);
    this.layer1Fixed = layer1Fixed;
    this.layer2Fixed = layer2Fixed;
    this.filter = filter;
    this.param = param;
    v1 = sourcePoints1;
    v2 = sourcePoints2;
    this.box = box;
}
 

/**
 * Return the maximum square distance among any pairwise point
 * distances in the P1 points of a list of point matches.
 * This is a rough estimate of the maximum spatial extent of a point
 * cloud that is used to find the 'widest' cloud.
 *
 * @param matches
 * @return
 */
final static private double squareP1LocalWidth( final List< PointMatch > matches )
{
	double dMax = 0;
	for ( int i = 0; i < matches.size(); ++i )
	{
		final PointMatch m1 = matches.get( i );
		for ( int j = i + 1; j < matches.size(); ++j )
		{
			final PointMatch m2 = matches.get( j );
			final double d = Point.squareLocalDistance( m1.getP1(), m2.getP1() );
			if ( d > dMax )
				dMax = d;
		}
	}
	return dMax;
}
 

/**
 * Constructor
 * 
 * Create a {@link PointMatch} with one weight.
 * 
 * @param p1 Point 1
 * @param p2 Point 2
 * @param weight Weight
 */
public PointMatchStitching(
		Point p1,
		Point p2,
		float weight,
		ComparePair pair )
{
	super ( p1, p2, weight );
	
	this.pair = pair;
}
 

private static double[] getWorldDeltaXAndYStandardDeviation(final List<PointMatch> pointMatchList) {
    final double[] deltaWorldX = new double[pointMatchList.size()];
    final double[] deltaWorldY = new double[pointMatchList.size()];
    for (int i = 0; i < pointMatchList.size(); i++) {
        final PointMatch pointMatch = pointMatchList.get(i);
        final Point p = pointMatch.getP1();
        final Point q = pointMatch.getP2();
        deltaWorldX[i] = p.getW()[0] - q.getW()[0];
        deltaWorldY[i] = p.getW()[1] - q.getW()[1];
    }
    return new double[] { calculateStandardDeviation(deltaWorldX), calculateStandardDeviation(deltaWorldY) };
}
 

/**
 * @param  matches  point match list in {@link Matches} form.
 *
 * @return the corresponding list of {@link PointMatch} objects.
 */
public static List<PointMatch> convertMatchesToPointMatchList(final Matches matches) {

    final double[] w = matches.getWs();

    final int pointMatchCount = w.length;
    final List<PointMatch> pointMatchList = new ArrayList<>(pointMatchCount);

    if (pointMatchCount > 0) {
        final double[][] p = matches.getPs();
        final double[][] q = matches.getQs();

        final int dimensionCount = p.length;

        for (int matchIndex = 0; matchIndex < pointMatchCount; matchIndex++) {

            final double[] pLocal = new double[dimensionCount];
            final double[] qLocal = new double[dimensionCount];

            for (int dimensionIndex = 0; dimensionIndex < dimensionCount; dimensionIndex++) {
                pLocal[dimensionIndex] = p[dimensionIndex][matchIndex];
                qLocal[dimensionIndex] = q[dimensionIndex][matchIndex];
            }

            pointMatchList.add(new PointMatch(new Point(pLocal), new Point(qLocal), w[matchIndex]));
        }
    }

    return pointMatchList;
}
 

private static Point getLocalPoint(final Point worldPoint,
                                   final TileSpec tileSpec)
        throws NoninvertibleModelException {
    final double[] world = worldPoint.getL();
    final double[] local = tileSpec.getLocalCoordinates(world[0], world[1], tileSpec.getMeshCellSize());
    return new Point(local);
}
 

private void verifyEquality(final String context,
                            final Point expected,
                            final Point actual) {

    final double[] expectedLocal = expected.getL();
    final double[] actualLocal = actual.getL();

    Assert.assertEquals("incorrect dimension size for " + context, expectedLocal.length, actualLocal.length);

    for (int i = 0; i < expectedLocal.length; i++) {
        Assert.assertEquals("incorrect value at index " + i + " of " + context,
                            expectedLocal[i], actualLocal[i], 0.0001);
    }

}
 

public double getDistance( final Point point2 )
{
	double distance = 0;
	final double[] a = getL();
	final double[] b = point2.getW();
	
	for ( int i = 0; i < getL().length; ++i )
	{
		final double tmp = a[ i ] - b[ i ];
		distance += tmp * tmp;
	}
	
	return Math.sqrt( distance );
}
 

public static <P extends Point & Leaf<P>> ArrayList< LocalCoordinateSystemPointDescriptor< P > > createLocalCoordinateSystemPointDescriptors( 
		final KDTree< P > tree, 
           final ArrayList< P > basisPoints, 
           final int numNeighbors,
           final boolean normalize )
{
	final NNearestNeighborSearch< P > nnsearch = new NNearestNeighborSearch< P >( tree );
	final ArrayList< LocalCoordinateSystemPointDescriptor< P > > descriptors = new ArrayList< LocalCoordinateSystemPointDescriptor< P > > ( );
	
	for ( final P point : basisPoints )
	{
		final ArrayList< P > neighbors = new ArrayList< P >();
		final P neighborList[] = nnsearch.findNNearestNeighbors( point, numNeighbors + 1 );
		
		// the first hit is always the point itself
		for ( int n = 1; n < neighborList.length; ++n )
		neighbors.add( neighborList[ n ] );
		
		try
		{
			descriptors.add( new LocalCoordinateSystemPointDescriptor<P>( point, neighbors, normalize ) );
		}
		catch ( NoSuitablePointsException e )
		{
			e.printStackTrace();
		}
	}
	
	return descriptors;
}
 

public double getDistance( final Point point2 )
{
	double distance = 0;
	final double[] a = getL();
	final double[] b = point2.getW();
	
	for ( int i = 0; i < getL().length; ++i )
	{
		final double tmp = a[ i ] - b[ i ];
		distance += tmp * tmp;
	}
	
	return Math.sqrt( distance );
}
 

public static <P extends Point & Leaf<P>> ArrayList< LocalCoordinateSystemPointDescriptor< P > > createLocalCoordinateSystemPointDescriptors( 
		final KDTree< P > tree, 
           final ArrayList< P > basisPoints, 
           final int numNeighbors,
           final boolean normalize )
{
	final NNearestNeighborSearch< P > nnsearch = new NNearestNeighborSearch< P >( tree );
	final ArrayList< LocalCoordinateSystemPointDescriptor< P > > descriptors = new ArrayList< LocalCoordinateSystemPointDescriptor< P > > ( );
	
	for ( final P point : basisPoints )
	{
		final ArrayList< P > neighbors = new ArrayList< P >();
		final P neighborList[] = nnsearch.findNNearestNeighbors( point, numNeighbors + 1 );
		
		// the first hit is always the point itself
		for ( int n = 1; n < neighborList.length; ++n )
		neighbors.add( neighborList[ n ] );
		
		try
		{
			descriptors.add( new LocalCoordinateSystemPointDescriptor<P>( point, neighbors, normalize ) );
		}
		catch ( NoSuitablePointsException e )
		{
			e.printStackTrace();
		}
	}
	
	return descriptors;
}
 

public static <P extends Point & Leaf<P>> ArrayList< ModelPointDescriptor< P > > createModelPointDescriptors( final KDTree< P > tree, 
              final ArrayList< P > basisPoints, 
              final int numNeighbors, 
              final TranslationInvariantModel<?> model, 
              final Matcher matcher, 
              final SimilarityMeasure similarityMeasure )
{
	final NNearestNeighborSearch< P > nnsearch = new NNearestNeighborSearch< P >( tree );
	final ArrayList< ModelPointDescriptor< P > > descriptors = new ArrayList< ModelPointDescriptor< P > > ( );
	
	for ( final P point : basisPoints )
	{
		final ArrayList< P > neighbors = new ArrayList< P >();
		final P neighborList[] = nnsearch.findNNearestNeighbors( point, numNeighbors + 1 );
		
		// the first hit is always the point itself
		for ( int n = 1; n < neighborList.length; ++n )
			neighbors.add( neighborList[ n ] );
		
		try
		{
			descriptors.add( new ModelPointDescriptor<P>( point, neighbors, model, similarityMeasure, matcher ) );
		}
		catch ( NoSuitablePointsException e )
		{
			e.printStackTrace();
		}
	}
	
	return descriptors;
}
 

/**
 * Detects ambigous (and duplicate) {@link PointMatch}es, i.e. if a {@link Point} corresponds with more than one other {@link Point}
 * @param matches - the {@link List} of {@link PointMatch}es
 * @return - the {@link ArrayList} containing the removed ambigous or duplicate {@link PointMatch}es 
 */
public static ArrayList<PointMatch> removeAmbigousMatches( final List<PointMatch> matches )
{
	final ArrayList<Integer> inconsistentCorrespondences = new ArrayList<Integer>();
	final ArrayList<PointMatch> ambigousMatches = new ArrayList<PointMatch>();
	
	for ( int i = 0; i < matches.size(); i++ )
	{
		final Point pointTarget = matches.get( i ).getP1();
		final Point pointReference = matches.get( i ).getP2();

		final ArrayList<Integer> inconsistent = getOccurences( pointTarget, pointReference, matches );
		
		if ( inconsistent.size() > 0 )
			for ( int index : inconsistent )
				if ( !inconsistentCorrespondences.contains( index ) )						
					inconsistentCorrespondences.add( index );
	}

	if ( inconsistentCorrespondences.size() > 0 )
	{
		Collections.sort( inconsistentCorrespondences );
					
		for ( int i = inconsistentCorrespondences.size() - 1; i >= 0; i-- )
		{
			// save the ambigous match
			final PointMatch pm = matches.get( (int)inconsistentCorrespondences.get(i) );				
			ambigousMatches.add( pm );
			
			// the cast to (int) is essential as otherwise he is looking to remove the Integer object that does not exist in the list 
			matches.remove( (int)inconsistentCorrespondences.get(i) );
		}
	}	
	
	return ambigousMatches;
}
 

@Override
public double getSimilarity( final ArrayList<PointMatch> matches )
{
	final int numDimensions = matches.get( 0 ).getP1().getL().length;
	
	double difference = 0;
	
	for ( final PointMatch match : matches )
		difference += Point.distance( match.getP1(), match.getP2() );
					
	return difference / (double)numDimensions;		
}
 

@Override
public double getSimilarity( final ArrayList<PointMatch> matches )
{
	final int numDimensions = matches.get( 0 ).getP1().getL().length;
	
	double difference = 0;

	for ( final PointMatch match : matches )
		difference += Point.squareDistance( match.getP1(), match.getP2() );
					
	return difference / (double)numDimensions;		
}
 

protected static void add( final Point p1, final Point p2 )
{
	final double[] l1 = p1.getL();
	final double[] w1 = p1.getW();
	final double[] l2 = p2.getL();
	final double[] w2 = p2.getW();
	
	for ( int d = 0; d < l1.length; ++d )
	{
		l1[ d ] += l2[ d ];
		w1[ d ] += w2[ d ];
	}
}
 

protected void applyTransform( final ArrayList<Point> points )
{
       final Transform3D trans = new Transform3D();
       trans.rotX( Math.toRadians( 30 ) );
       
       final AffineModel3D model = TransformUtils.getAffineModel3D( trans );
       
       for ( final Point p : points )
       {
       	model.apply( p.getL() );
       	model.apply( p.getW() );
       }        			
}
 

public static <P extends Point> ArrayList< VirtualPointNode< P > > createVirtualNodeList( final ArrayList<P> points )
{
	final ArrayList< VirtualPointNode< P > > nodeList = new ArrayList< VirtualPointNode< P > >();
	
	for ( final P point : points )
		nodeList.add( new VirtualPointNode<P>( point ) );
	
	return nodeList;
}
 

public static < P extends Point > ArrayList< ModelPointDescriptor< P > > createModelPointDescriptors( final KDTree< VirtualPointNode< P > > tree, 
                                                                                               final ArrayList< VirtualPointNode< P > > basisPoints, 
                                                                                               final int numNeighbors, 
                                                                                               final TranslationInvariantModel<?> model, 
                                                                                               final Matcher matcher, 
                                                                                               final SimilarityMeasure similarityMeasure )
{
	final NNearestNeighborSearch< VirtualPointNode< P > > nnsearch = new NNearestNeighborSearch< VirtualPointNode< P > >( tree );
	final ArrayList< ModelPointDescriptor< P > > descriptors = new ArrayList< ModelPointDescriptor< P > > ( );
	
	for ( final VirtualPointNode< P > p : basisPoints )
	{
		final ArrayList< P > neighbors = new ArrayList< P >();
		final VirtualPointNode< P > neighborList[] = nnsearch.findNNearestNeighbors( p, numNeighbors + 1 );
		
		// the first hit is always the point itself
		for ( int n = 1; n < neighborList.length; ++n )
			neighbors.add( neighborList[ n ].getPoint() );
		
		try
		{
			descriptors.add( new ModelPointDescriptor<P>( p.getPoint(), neighbors, model, similarityMeasure, matcher ) );
		}
		catch ( NoSuitablePointsException e )
		{
			e.printStackTrace();
		}
	}
		
	return descriptors;
}
 

public static < P extends Point > ArrayList< LocalCoordinateSystemPointDescriptor< P > > createLocalCoordinateSystemPointDescriptors( 
		final KDTree< VirtualPointNode< P > > tree, 
           final ArrayList< VirtualPointNode< P > > basisPoints, 
           final int numNeighbors,
           final boolean normalize )
{
	final NNearestNeighborSearch< VirtualPointNode< P > > nnsearch = new NNearestNeighborSearch< VirtualPointNode< P > >( tree );
	final ArrayList< LocalCoordinateSystemPointDescriptor< P > > descriptors = new ArrayList< LocalCoordinateSystemPointDescriptor< P > > ( );
	
	for ( final VirtualPointNode< P > p : basisPoints )
	{
		final ArrayList< P > neighbors = new ArrayList< P >();
		final VirtualPointNode< P > neighborList[] = nnsearch.findNNearestNeighbors( p, numNeighbors + 1 );
		
		// the first hit is always the point itself
		for ( int n = 1; n < neighborList.length; ++n )
		neighbors.add( neighborList[ n ].getPoint() );
		
		try
		{
			descriptors.add( new LocalCoordinateSystemPointDescriptor<P>( p.getPoint(), neighbors, normalize ) );
		}
		catch ( NoSuitablePointsException e )
		{
			e.printStackTrace();
		}
	}
	
	return descriptors;
}
 

protected static boolean isCorrect( Point a, Point b )
{
	if ( a instanceof LinkedPoint )
	{
		if ( ((LinkedPoint<Point>)a).getLinkedObject() == b  )
			return true;
		else
			return false;
	}
	else if ( b instanceof LinkedPoint )
	{
		if ( ((LinkedPoint<Point>)b).getLinkedObject() == a  )
			return true;
		else
			return false;
	}
	else
	{
		return false;
	}
	
	/*
	double[] a1 = a.getL();
	double[] b1 = b.getL();
	
	if ( a1[ 0 ] == b1[ 0 ] && a1[ 1 ] == b1[ 1 ] && a1[ 2 ] == b1[ 2 ] )
		return true;
	else
		return false;
	*/
}