/**
 * Copyright (c) 2011, The University of Southampton and the individual contributors.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *   *  Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *
 *   *  Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *
 *   *  Neither the name of the University of Southampton nor the names of its
 *      contributors may be used to endorse or promote products derived from this
 *      software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package org.openimaj.image.feature.local.interest;

import java.io.IOException;
import java.util.ArrayList;
import java.util.List;

import org.apache.log4j.BasicConfigurator;
import org.apache.log4j.Level;
import org.apache.log4j.Logger;
import org.openimaj.image.DisplayUtilities;
import org.openimaj.image.FImage;
import org.openimaj.image.ImageUtilities;
import org.openimaj.image.MBFImage;
import org.openimaj.image.colour.RGBColour;
import org.openimaj.image.pixel.FValuePixel;
import org.openimaj.image.pixel.Pixel;
import org.openimaj.image.processing.convolution.FConvolution;
import org.openimaj.image.processing.convolution.FGaussianConvolve;
import org.openimaj.image.processing.resize.ResizeProcessor;
import org.openimaj.image.processing.transform.FProjectionProcessor;
import org.openimaj.math.geometry.point.Point2dImpl;
import org.openimaj.math.geometry.shape.Ellipse;
import org.openimaj.math.geometry.shape.EllipseUtilities;
import org.openimaj.math.geometry.shape.Rectangle;
import org.openimaj.math.matrix.EigenValueVectorPair;
import org.openimaj.math.matrix.MatrixUtils;

import Jama.Matrix;

/**
 * Using an underlying feature detector, adapt the ellipse detected to result in
 * a more stable region according to work by
 * http://www.robots.ox.ac.uk/~vgg/research/affine/
 *
 * @author Sina Samangooei (ss@ecs.soton.ac.uk)
 *
 */
public class AffineAdaption implements MultiscaleInterestPointDetector<EllipticInterestPointData> {
        private static final FImage LAPLACIAN_KERNEL = new FImage(new float[][] { { 2, 0, 2 }, { 0, -8, 0 }, { 2, 0, 2 } });
        private static final FConvolution LAPLACIAN_KERNEL_CONV = new FConvolution(LAPLACIAN_KERNEL);
        // private static final FImage DX_KERNEL = new FImage(new float[][] {{-1, 0,
        // 1}});
        // private static final FImage DY_KERNEL = new FImage(new float[][] {{-1},
        // {0}, {1}});

        static Logger logger = Logger.getLogger(AffineAdaption.class);
        static {

                BasicConfigurator.configure();
                logger.setLevel(Level.INFO);
        }

        private AbstractStructureTensorIPD internal;
        private AbstractStructureTensorIPD initial;
        private List<EllipticInterestPointData> points;

        private IPDSelectionMode initialMode;

        private boolean fastDifferentiationScale = false;

        /**
         * instatiate with a {@link HarrisIPD} detector with scales 2.0f and 2.0f *
         * 1.4f. Selection 100 points
         */
        public AffineAdaption() {
                this(new HarrisIPD(2.0f, 2.0f * 1.4f), new IPDSelectionMode.Count(100));
        }

        /**
         * specify the internal detector and the selection mode
         *
         * @param internal
         * @param initialSelectionMode
         */
        public AffineAdaption(AbstractStructureTensorIPD internal, IPDSelectionMode initialSelectionMode) {
                this(internal, internal.clone(), initialSelectionMode);
        }

        /**
         * set both the internal and intitial feature detectors (possibly different
         * settings of the same detector) and a selection mode
         *
         * @param internal
         * @param initial
         * @param initialSelectionMode
         */
        public AffineAdaption(AbstractStructureTensorIPD internal, AbstractStructureTensorIPD initial,
                        IPDSelectionMode initialSelectionMode)
        {
                this.internal = internal;
                this.initial = initial;

                this.initialMode = initialSelectionMode;
                this.points = new ArrayList<EllipticInterestPointData>();
        }

        @Override
        public void findInterestPoints(FImage image) {
                findInterestPoints(image, image.getBounds());
        }

        @Override
        public void findInterestPoints(FImage image, Rectangle window) {
                this.points = new ArrayList<EllipticInterestPointData>();
                initial.findInterestPoints(image, window);
                // if(logger.getLevel() == Level.INFO)
                // initial.printStructureTensorStats();
                final List<InterestPointData> a = initialMode.selectPoints(initial);

                logger.info("Found " + a.size() + " features at sd/si: " + initial.detectionScale + "/"
                                + initial.integrationScale);

                for (final InterestPointData d : a) {
                        final EllipticInterestPointData kpt = new EllipticInterestPointData();
                        // InterestPointData d = new InterestPointData();
                        // d.x = 102;
                        // d.y = 396;
                        kpt.scale = initial.getIntegrationScale();
                        kpt.x = d.x;
                        kpt.y = d.y;

                        final boolean converge = calcAffineAdaptation(image, kpt, internal.clone());
                        if (converge)
                        {
                                logger.debug("Keypoint at: " + d.x + ", " + d.y);
                                logger.debug("... converged: " + converge);
                                this.points.add(kpt);
                        }

                }
        }

        @Override
        public List<EllipticInterestPointData> getInterestPoints(int npoints) {
                if (points == null)
                        return null;
                if (npoints < 0)
                        npoints = this.points.size();
                return this.points.subList(0, npoints < this.points.size() ? npoints : this.points.size());
        }

        @Override
        public List<EllipticInterestPointData> getInterestPoints(float threshold) {
                final List<EllipticInterestPointData> validPoints = new ArrayList<EllipticInterestPointData>();
                for (final EllipticInterestPointData point : this.points) {
                        if (point.score > threshold) {
                                validPoints.add(point);
                        }
                }
                return validPoints;
        }

        @Override
        public List<EllipticInterestPointData> getInterestPoints() {
                return this.points;
        }

        @Override
        public void setDetectionScale(float detectionScaleVariance) {
                this.initial.setDetectionScale(detectionScaleVariance);
        }

        public void setIntegrationScale(float integrationScaleVariance) {
                this.initial.setIntegrationScale(integrationScaleVariance);
        }

        /*
         * Calculates second moments matrix in point p
         */
        // Matrix calcSecondMomentMatrix(final FImage dx2, final FImage dxy, final
        // FImage dy2, Pixel p) {
        // int x = p.x;
        // int y = p.y;
        //
        // Matrix M = new Matrix(2, 2);
        // M.set(0, 0, dx2.pixels[y][x]);
        // M.set(0, 1, dxy.pixels[y][x]);
        // M.set(1, 0, dxy.pixels[y][x]);
        // M.set(1, 1, dy2.pixels[y][x]);
        //
        // return M;
        // }
        Matrix calcSecondMomentMatrix(AbstractStructureTensorIPD ipd, int x, int y) {

                return ipd.getSecondMomentsAt(x, y);
        }

        /*
         * Performs affine adaptation
         */
        boolean calcAffineAdaptation(final FImage fimage, EllipticInterestPointData kpt, AbstractStructureTensorIPD ipd) {
                // DisplayUtilities.createNamedWindow("warp", "Warped Image ROI",true);
                final Matrix transf = new Matrix(2, 3); // Transformation matrix
                Point2dImpl c = new Point2dImpl(); // Transformed point
                final Point2dImpl p = new Point2dImpl(); // Image point

                Matrix U = Matrix.identity(2, 2); // Normalization matrix

                final Matrix Mk = U.copy();
                FImage img_roi, warpedImg = new FImage(1, 1);
                float Qinv = 1, q, si = kpt.scale; // sd = 0.75f * si;
                final float kptSize = 2 * 3 * kpt.scale;
                boolean divergence = false, convergence = false;
                int i = 0;

                // Coordinates in image
                int py = (int) kpt.y;
                int px = (int) kpt.x;

                // Roi coordinates
                int roix, roiy;

                // Coordinates in U-trasformation
                int cx = px;
                int cy = py;
                int cxPr = cx;
                int cyPr = cy;

                float radius = kptSize / 2 * 1.4f;
                float half_width, half_height;

                Rectangle roi;

                // Affine adaptation
                while (i <= 10 && !divergence && !convergence)
                {
                        // Transformation matrix
                        MatrixUtils.zero(transf);
                        transf.setMatrix(0, 1, 0, 1, U);

                        kpt.setTransform(U);

                        final Rectangle boundingBox = new Rectangle();

                        final double ac_b2 = U.det();
                        boundingBox.width = (float) Math.ceil(U.get(1, 1) / ac_b2 * 3 * si * 1.4);
                        boundingBox.height = (float) Math.ceil(U.get(0, 0) / ac_b2 * 3 * si * 1.4);

                        // Create window around interest point
                        half_width = Math.min(Math.min(fimage.width - px - 1, px), boundingBox.width);
                        half_height = Math.min(Math.min(fimage.height - py - 1, py), boundingBox.height);

                        if (half_width <= 0 || half_height <= 0)
                                return divergence;

                        roix = Math.max(px - (int) boundingBox.width, 0);
                        roiy = Math.max(py - (int) boundingBox.height, 0);
                        roi = new Rectangle(roix, roiy, px - roix + half_width + 1, py - roiy + half_height + 1);

                        // create ROI
                        img_roi = fimage.extractROI(roi);

                        // Point within the ROI
                        p.x = px - roix;
                        p.y = py - roiy;

                        // Find coordinates of square's angles to find size of warped
                        // ellipse's bounding box
                        final float u00 = (float) U.get(0, 0);
                        final float u01 = (float) U.get(0, 1);
                        final float u10 = (float) U.get(1, 0);
                        final float u11 = (float) U.get(1, 1);

                        final float minx = u01 * img_roi.height < 0 ? u01 * img_roi.height : 0;
                        final float miny = u10 * img_roi.width < 0 ? u10 * img_roi.width : 0;
                        final float maxx = (u00 * img_roi.width > u00 * img_roi.width + u01 * img_roi.height ? u00
                                        * img_roi.width : u00 * img_roi.width + u01 * img_roi.height) - minx;
                        final float maxy = (u11 * img_roi.width > u10 * img_roi.width + u11 * img_roi.height ? u11
                                        * img_roi.height : u10 * img_roi.width + u11 * img_roi.height) - miny;

                        // Shift
                        transf.set(0, 2, -minx);
                        transf.set(1, 2, -miny);

                        if (maxx >= 2 * radius + 1 && maxy >= 2 * radius + 1)
                        {
                                // Size of normalized window must be 2*radius
                                // Transformation
                                FImage warpedImgRoi;
                                final FProjectionProcessor proc = new FProjectionProcessor();
                                proc.setMatrix(transf);
                                img_roi.accumulateWith(proc);
                                warpedImgRoi = proc.performProjection(0, (int) maxx, 0, (int) maxy, null);

                                // DisplayUtilities.displayName(warpedImgRoi.clone().normalise(),
                                // "warp");

                                // Point in U-Normalized coordinates
                                c = p.transform(U);
                                cx = (int) (c.x - minx);
                                cy = (int) (c.y - miny);

                                if (warpedImgRoi.height > 2 * radius + 1 && warpedImgRoi.width > 2 * radius + 1)
                                {
                                        // Cut around normalized patch
                                        roix = (int) Math.max(cx - Math.ceil(radius), 0.0);
                                        roiy = (int) Math.max(cy - Math.ceil(radius), 0.0);
                                        roi = new Rectangle(roix, roiy,
                                                        cx - roix + (float) Math.min(Math.ceil(radius), warpedImgRoi.width - cx - 1) + 1,
                                                        cy - roiy + (float) Math.min(Math.ceil(radius), warpedImgRoi.height - cy - 1) + 1);
                                        warpedImg = warpedImgRoi.extractROI(roi);

                                        // Coordinates in cutted ROI
                                        cx = cx - roix;
                                        cy = cy - roiy;
                                } else {
                                        warpedImg.internalAssign(warpedImgRoi);
                                }

                                if (logger.getLevel() == Level.DEBUG) {
                                        displayCurrentPatch(img_roi.clone().normalise(), p.x, p.y, warpedImg.clone().normalise(), cx, cy, U,
                                                        si * 3);
                                }

                                // Integration Scale selection
                                si = selIntegrationScale(warpedImg, si, new Pixel(cx, cy));

                                // Differentation scale selection
                                if (fastDifferentiationScale) {
                                        ipd = selDifferentiationScaleFast(warpedImg, ipd, si, new Pixel(cx, cy));
                                }
                                else {
                                        ipd = selDifferentiationScale(warpedImg, ipd, si, new Pixel(cx, cy));
                                }

                                if (ipd.maxima.size() == 0) {
                                        divergence = true;
                                        continue;
                                }
                                // Spatial Localization
                                cxPr = cx; // Previous iteration point in normalized window
                                cyPr = cy;
                                //
                                // float cornMax = 0;
                                // for (int j = 0; j < 3; j++)
                                // {
                                // for (int t = 0; t < 3; t++)
                                // {
                                // float dx2 = Lxm2smooth.pixels[cyPr - 1 + j][cxPr - 1 + t];
                                // float dy2 = Lym2smooth.pixels[cyPr - 1 + j][cxPr - 1 + t];
                                // float dxy = Lxmysmooth.pixels[cyPr - 1 + j][cxPr - 1 + t];
                                // float det = dx2 * dy2 - dxy * dxy;
                                // float tr = dx2 + dy2;
                                // float cornerness = (float) (det - (0.04 * Math.pow(tr, 2)));
                                //
                                // if (cornerness > cornMax) {
                                // cornMax = cornerness;
                                // cx = cxPr - 1 + t;
                                // cy = cyPr - 1 + j;
                                // }
                                // }
                                // }

                                final FValuePixel max = ipd.findMaximum(new Rectangle(cxPr - 1, cyPr - 1, 3, 3));
                                cx = max.x;
                                cy = max.y;

                                // Transform point in image coordinates
                                p.x = px;
                                p.y = py;

                                // Displacement vector
                                c.x = cx - cxPr;
                                c.y = cy - cyPr;

                                // New interest point location in image
                                p.translate(c.transform(U.inverse()));
                                px = (int) p.x;
                                py = (int) p.y;

                                q = calcSecondMomentSqrt(ipd, new Pixel(cx, cy), Mk);

                                final float ratio = 1 - q;

                                // if ratio == 1 means q == 0 and one axes equals to 0
                                if (!Float.isNaN(ratio) && ratio != 1)
                                {
                                        // Update U matrix
                                        U = U.times(Mk);

                                        Matrix uVal, uV;
                                        // EigenvalueDecomposition ueig = U.eig();
                                        final EigenValueVectorPair ueig = MatrixUtils.symmetricEig2x2(U);
                                        uVal = ueig.getValues();
                                        uV = ueig.getVectors();

                                        Qinv = normMaxEval(U, uVal, uV);

                                        // Keypoint doesn't converge
                                        if (Qinv >= 6) {
                                                logger.debug("QInverse too large, feature too edge like, affine divergence!");
                                                divergence = true;
                                        } else if (ratio <= 0.05) { // Keypoint converges
                                                convergence = true;

                                                // Set transformation matrix
                                                MatrixUtils.zero(transf);
                                                transf.setMatrix(0, 1, 0, 1, U);
                                                // The order here matters, setTransform uses the x and y
                                                // to calculate a new ellipse
                                                kpt.x = px;
                                                kpt.y = py;
                                                kpt.scale = si;
                                                kpt.setTransform(U);
                                                kpt.score = max.value;

                                                // ax1 = (float) (1 / Math.abs(uVal.get(1, 1)) * 3 *
                                                // si);
                                                // ax2 = (float) (1 / Math.abs(uVal.get(0, 0)) * 3 *
                                                // si);
                                                // phi = Math.atan(uV.get(1, 1) / uV.get(0, 1));
                                                // kpt.axes = new Point2dImpl(ax1, ax2);
                                                // kpt.phi = phi;
                                                // kpt.centre = new Pixel(px, py);
                                                // kpt.si = si;
                                                // kpt.size = 2 * 3 * si;

                                        } else {
                                                radius = (float) (3 * si * 1.4);
                                        }
                                } else {
                                        logger.debug("QRatio was close to 0, affine divergence!");
                                        divergence = true;
                                }
                        } else {
                                logger.debug("Window size has grown too fast, scale divergence!");
                                divergence = true;
                        }

                        ++i;
                }
                if (!divergence && !convergence) {
                        logger.debug("Reached max iterations!");
                }
                return convergence;
        }

        private void displayCurrentPatch(FImage unwarped, float unwarpedx, float unwarpedy, FImage warped, int warpedx,
                        int warpedy, Matrix transform, float scale)
        {
                DisplayUtilities.createNamedWindow("warpunwarp", "Warped and Unwarped Image", true);
                logger.debug("Displaying patch");
                final float resizeScale = 5f;
                final float warppedPatchScale = resizeScale;
                final ResizeProcessor patchSizer = new ResizeProcessor(resizeScale);
                final FImage warppedPatchGrey = warped.process(patchSizer);
                MBFImage warppedPatch = new MBFImage(warppedPatchGrey.clone(), warppedPatchGrey.clone(), warppedPatchGrey.clone());
                float x = warpedx * warppedPatchScale;
                float y = warpedy * warppedPatchScale;
                final float r = scale * warppedPatchScale;

                warppedPatch.createRenderer().drawShape(new Ellipse(x, y, r, r, 0), RGBColour.RED);
                warppedPatch.createRenderer().drawPoint(new Point2dImpl(x, y), RGBColour.RED, 3);

                final FImage unwarppedPatchGrey = unwarped.clone();
                MBFImage unwarppedPatch = new MBFImage(unwarppedPatchGrey.clone(), unwarppedPatchGrey.clone(),
                                unwarppedPatchGrey.clone());
                unwarppedPatch = unwarppedPatch.process(patchSizer);
                final float unwarppedPatchScale = resizeScale;

                x = unwarpedx * unwarppedPatchScale;
                y = unwarpedy * unwarppedPatchScale;
                // Matrix sm = state.selected.secondMoments;
                // float scale = state.selected.scale * unwarppedPatchScale;
                // Ellipse e = EllipseUtilities.ellipseFromSecondMoments(x, y, sm,
                // scale*2);
                final Ellipse e = EllipseUtilities.fromTransformMatrix2x2(transform, x, y, scale * unwarppedPatchScale);

                unwarppedPatch.createRenderer().drawShape(e, RGBColour.BLUE);
                unwarppedPatch.createRenderer().drawPoint(new Point2dImpl(x, y), RGBColour.RED, 3);
                // give the patch a border (10px, black)
                warppedPatch = warppedPatch.padding(5, 5, RGBColour.BLACK);
                unwarppedPatch = unwarppedPatch.padding(5, 5, RGBColour.BLACK);

                final MBFImage displayArea = warppedPatch.newInstance(warppedPatch.getWidth() * 2, warppedPatch.getHeight());
                displayArea.createRenderer().drawImage(warppedPatch, 0, 0);
                displayArea.createRenderer().drawImage(unwarppedPatch, warppedPatch.getWidth(), 0);
                DisplayUtilities.displayName(displayArea, "warpunwarp");
                logger.debug("Done");
        }

        /*
         * Selects the integration scale that maximize LoG in point c
         */
        float selIntegrationScale(final FImage image, float si, Pixel c) {
                FImage L;
                final int cx = c.x;
                final int cy = c.y;
                float maxLap = -Float.MAX_VALUE;
                float maxsx = si;
                float sigma, sigma_prev = 0;

                L = image.clone();
                /*
                 * Search best integration scale between previous and successive layer
                 */
                for (float u = 0.7f; u <= 1.41; u += 0.1)
                {
                        final float sik = u * si;
                        sigma = (float) Math.sqrt(Math.pow(sik, 2) - Math.pow(sigma_prev, 2));

                        L.processInplace(new FGaussianConvolve(sigma, 3));

                        sigma_prev = sik;
                        // Lap = L.process(LAPLACIAN_KERNEL_CONV);

                        final float lapVal = sik * sik * Math.abs(LAPLACIAN_KERNEL_CONV.responseAt(cx, cy, L));
                        // float lapVal = sik * sik * Math.abs(Lap.pixels[cy][cx]);

                        if (lapVal >= maxLap)
                        {
                                maxLap = lapVal;
                                maxsx = sik;
                        }
                }
                return maxsx;
        }

        /*
         * Calculates second moments matrix square root
         */
        float calcSecondMomentSqrt(AbstractStructureTensorIPD ipd, Pixel p, Matrix Mk)
        {
                Matrix M, V, eigVal, Vinv;

                M = calcSecondMomentMatrix(ipd, p.x, p.y);

                /* *
                 * M = V * D * V.inv() V has eigenvectors as columns D is a diagonal
                 * Matrix with eigenvalues as elements V.inv() is the inverse of V
                 */
                // EigenvalueDecomposition meig = M.eig();
                final EigenValueVectorPair meig = MatrixUtils.symmetricEig2x2(M);
                eigVal = meig.getValues();
                V = meig.getVectors();

                // V = V.transpose();
                Vinv = V.inverse();

                final double eval1 = Math.sqrt(eigVal.get(0, 0));
                eigVal.set(0, 0, eval1);
                final double eval2 = Math.sqrt(eigVal.get(1, 1));
                eigVal.set(1, 1, eval2);

                // square root of M
                Mk.setMatrix(0, 1, 0, 1, V.times(eigVal).times(Vinv));

                // return q isotropic measure
                return (float) (Math.min(eval1, eval2) / Math.max(eval1, eval2));
        }

        float normMaxEval(Matrix U, Matrix uVal, Matrix uVec) {
                /* *
                 * Decomposition: U = V * D * V.inv() V has eigenvectors as columns D is
                 * a diagonal Matrix with eigenvalues as elements V.inv() is the inverse
                 * of V
                 */
                // uVec = uVec.transpose();
                final Matrix uVinv = uVec.inverse();

                // Normalize min eigenvalue to 1 to expand patch in the direction of min
                // eigenvalue of U.inv()
                final double uval1 = uVal.get(0, 0);
                final double uval2 = uVal.get(1, 1);

                if (Math.abs(uval1) < Math.abs(uval2))
                {
                        uVal.set(0, 0, 1);
                        uVal.set(1, 1, uval2 / uval1);

                } else
                {
                        uVal.set(1, 1, 1);
                        uVal.set(0, 0, uval1 / uval2);
                }

                // U normalized
                U.setMatrix(0, 1, 0, 1, uVec.times(uVal).times(uVinv));

                return (float) (Math.max(Math.abs(uVal.get(0, 0)), Math.abs(uVal.get(1, 1))) / Math.min(Math.abs(uVal.get(0, 0)),
                                Math.abs(uVal.get(1, 1)))); // define the direction of warping
        }

        /*
         * Selects diffrentiation scale
         */
        AbstractStructureTensorIPD
        selDifferentiationScale(FImage img, AbstractStructureTensorIPD ipdToUse, float si, Pixel c)
        {
                AbstractStructureTensorIPD best = null;
                float s = 0.5f;
                float sigma_prev = 0, sigma;

                FImage L;

                double qMax = 0;

                L = img.clone();

                final AbstractStructureTensorIPD ipd = ipdToUse.clone();

                while (s <= 0.751)
                {
                        Matrix M;
                        final float sd = s * si;

                        // Smooth previous smoothed image L
                        sigma = (float) Math.sqrt(Math.pow(sd, 2) - Math.pow(sigma_prev, 2));

                        L.processInplace(new FGaussianConvolve(sigma, 3));
                        sigma_prev = sd;

                        // X and Y derivatives
                        ipd.setDetectionScale(sd);
                        ipd.setIntegrationScale(si);
                        ipd.setImageBlurred(true);

                        ipd.findInterestPoints(L);
                        // FImage Lx = L.process(new FConvolution(DX_KERNEL.multiply(sd)));
                        // FImage Ly = L.process(new FConvolution(DY_KERNEL.multiply(sd)));
                        //
                        // FGaussianConvolve gauss = new FGaussianConvolve(si, 3);
                        //
                        // FImage Lxm2 = Lx.multiply(Lx);
                        // dx2 = Lxm2.process(gauss);
                        //
                        // FImage Lym2 = Ly.multiply(Ly);
                        // dy2 = Lym2.process(gauss);
                        //
                        // FImage Lxmy = Lx.multiply(Ly);
                        // dxy = Lxmy.process(gauss);

                        M = calcSecondMomentMatrix(ipd, c.x, c.y);

                        // calc eigenvalues
                        // EigenvalueDecomposition meig = M.eig();
                        final EigenValueVectorPair meig = MatrixUtils.symmetricEig2x2(M);
                        final Matrix eval = meig.getValues();
                        final double eval1 = Math.abs(eval.get(0, 0));
                        final double eval2 = Math.abs(eval.get(1, 1));
                        final double q = Math.min(eval1, eval2) / Math.max(eval1, eval2);

                        if (q >= qMax) {
                                qMax = q;
                                best = ipd.clone();
                        }

                        s += 0.05;
                }
                return best;
        }

        AbstractStructureTensorIPD selDifferentiationScaleFast(FImage img, AbstractStructureTensorIPD ipd, float si, Pixel c)
        {
                final AbstractStructureTensorIPD best = ipd.clone();
                final float s = 0.75f;
                float sigma;
                FImage L;
                L = img.clone();
                final float sd = s * si;

                // Smooth previous smoothed image L
                sigma = sd;

                L.processInplace(new FGaussianConvolve(sigma, 3));

                // X and Y derivatives
                best.setDetectionScale(sd);
                best.setIntegrationScale(si);
                best.setImageBlurred(true);

                best.findInterestPoints(L);

                // M = calcSecondMomentMatrix(best, c.x, c.y);

                // EigenValueVectorPair meig = MatrixUtils.symmetricEig2x2(M);
                // Matrix eval = meig.getD();
                // double eval1 = Math.abs(eval.get(0, 0));
                // double eval2 = Math.abs(eval.get(1, 1));

                return best;
        }

        // void calcAffineCovariantRegions(final Matrix image, final
        // vector<KeyPoint> & keypoints,
        // vector<Elliptic_KeyPoint> & affRegions, string detector_type)
        // {
        //
        // for (size_t i = 0; i < keypoints.size(); ++i)
        // {
        // KeyPoint kp = keypoints[i];
        // Elliptic_KeyPoint ex(kp.pt, 0, Size_<float> (kp.size / 2, kp.size / 2),
        // kp.size,
        // kp.size / 6);
        //
        // if (calcAffineAdaptation(image, ex))
        // affRegions.push_back(ex);
        //
        // }
        // //Erase similar keypoint
        // float maxDiff = 4;
        // Matrix colorimg;
        // for (size_t i = 0; i < affRegions.size(); i++)
        // {
        // Elliptic_KeyPoint kp1 = affRegions[i];
        // for (size_t j = i+1; j < affRegions.size(); j++){
        //
        // Elliptic_KeyPoint kp2 = affRegions[j];
        //
        // if(norm(kp1.centre-kp2.centre)<=maxDiff){
        // double phi1, phi2;
        // Size axes1, axes2;
        // double si1, si2;
        // phi1 = kp1.phi;
        // phi2 = kp2.phi;
        // axes1 = kp1.axes;
        // axes2 = kp2.axes;
        // si1 = kp1.si;
        // si2 = kp2.si;
        // if(Math.abs(phi1-phi2)<15 && Math.max(si1,si2)/Math.min(si1,si2)<1.4 &&
        // axes1.width-axes2.width<5 && axes1.height-axes2.height<5){
        // affRegions.erase(affRegions.begin()+j);
        // j--;
        // }
        // }
        // }
        // }
        // }

        // void calcAffineCovariantDescriptors(final Ptr<DescriptorExtractor>&
        // dextractor, final Mat& img,
        // vector<Elliptic_KeyPoint>& affRegions, Mat& descriptors)
        // {
        //
        // assert(!affRegions.empty());
        // int size = dextractor->descriptorSize();
        // int type = dextractor->descriptorType();
        // descriptors = Mat(Size(size, affRegions.size()), type);
        // descriptors.setTo(0);
        //
        // int i = 0;
        //
        // for (vector<Elliptic_KeyPoint>::iterator it = affRegions.begin(); it <
        // affRegions.end(); ++it)
        // {
        // Point p = it->centre;
        //
        // Mat_<float> size(2, 1);
        // size(0, 0) = size(1, 0) = it->size;
        //
        // //U matrix
        // Matrix transf = it->transf;
        // Mat_<float> U(2, 2);
        // U.setTo(0);
        // Matrix col0 = U.col(0);
        // transf.col(0).copyTo(col0);
        // Matrix col1 = U.col(1);
        // transf.col(1).copyTo(col1);
        //
        // float radius = it->size / 2;
        // float si = it->si;
        //
        // Size_<float> boundingBox;
        //
        // double ac_b2 = determinant(U);
        // boundingBox.width = ceil(U.get(1, 1)/ac_b2 * 3 * si );
        // boundingBox.height = ceil(U.get(0, 0)/ac_b2 * 3 * si );
        //
        // //Create window around interest point
        // float half_width = Math.min((float) Math.min(img.width - p.x-1, p.x),
        // boundingBox.width);
        // float half_height = Math.min((float) Math.min(img.height - p.y-1, p.y),
        // boundingBox.height);
        // float roix = max(p.x - (int) boundingBox.width, 0);
        // float roiy = max(p.y - (int) boundingBox.height, 0);
        // Rect roi = Rect(roix, roiy, p.x - roix + half_width+1, p.y - roiy +
        // half_height+1);
        //
        // Matrix img_roi = img(roi);
        //
        // size(0, 0) = img_roi.width;
        // size(1, 0) = img_roi.height;
        //
        // size = U * size;
        //
        // Matrix transfImgRoi, transfImg;
        // warpAffine(img_roi, transfImgRoi, transf, Size(ceil(size(0, 0)),
        // ceil(size(1, 0))),
        // INTER_AREA, BORDER_DEFAULT);
        //
        //
        // Mat_<float> c(2, 1); //Transformed point
        // Mat_<float> pt(2, 1); //Image point
        // //Point within the Roi
        // pt(0, 0) = p.x - roix;
        // pt(1, 0) = p.y - roiy;
        //
        // //Point in U-Normalized coordinates
        // c = U * pt;
        // float cx = c(0, 0);
        // float cy = c(1, 0);
        //
        //
        // //Cut around point to have patch of 2*keypoint->size
        //
        // roix = Math.max(cx - radius, 0.f);
        // roiy = Math.max(cy - radius, 0.f);
        //
        // roi = Rect(roix, roiy, Math.min(cx - roix + radius, size(0, 0)),
        // Math.min(cy - roiy + radius, size(1, 0)));
        // transfImg = transfImgRoi(roi);
        //
        // cx = c(0, 0) - roix;
        // cy = c(1, 0) - roiy;
        //
        // Matrix tmpDesc;
        // KeyPoint kp(Point(cx, cy), it->size);
        //
        // vector<KeyPoint> k(1, kp);
        //
        // transfImg.convertTo(transfImg, CV_8U);
        // dextractor->compute(transfImg, k, tmpDesc);
        //
        // for (int j = 0; j < tmpDesc.width; j++)
        // {
        // descriptors.get(i, j) = tmpDesc.get(0, j);
        // }
        //
        // i++;
        //
        // }
        //
        // }

        /**
         * an example run
         *
         * @param args
         * @throws IOException
         */
        public static void main(String[] args) throws IOException {
                final float sd = 5;
                final float si = 1.4f * sd;
                final HessianIPD ipd = new HessianIPD(sd, si);
                final FImage img = ImageUtilities.readF(AffineAdaption.class
                                .getResourceAsStream("/org/openimaj/image/data/sinaface.jpg"));

                // img = img.multiply(255f);

                // ipd.findInterestPoints(img);
                // List<InterestPointData> a = ipd.getInterestPoints(1F/(256*256));
                //
                // System.out.println("Found " + a.size() + " features");
                //
                // AffineAdaption adapt = new AffineAdaption();
                // EllipticKeyPoint kpt = new EllipticKeyPoint();
                final MBFImage outImg = new MBFImage(img.clone(), img.clone(), img.clone());
                // for (InterestPointData d : a) {
                //
                // // InterestPointData d = new InterestPointData();
                // // d.x = 102;
                // // d.y = 396;
                // logger.info("Keypoint at: " + d.x + ", " + d.y);
                // kpt.si = si;
                // kpt.centre = new Pixel(d.x, d.y);
                // kpt.size = 2 * 3 * kpt.si;
                //
                // boolean converge = adapt.calcAffineAdaptation(img, kpt);
                // if(converge)
                // {
                // outImg.drawShape(new
                // Ellipse(kpt.centre.x,kpt.centre.y,kpt.axes.getX(),kpt.axes.getY(),kpt.phi),
                // RGBColour.BLUE);
                // outImg.drawPoint(kpt.centre, RGBColour.RED,3);
                // }
                //
                //
                //
                // logger.info("... converged: "+ converge);
                // }
                final AffineAdaption adapt = new AffineAdaption(ipd, new IPDSelectionMode.Count(100));
                adapt.findInterestPoints(img);
                final InterestPointVisualiser<Float[], MBFImage> ipv = InterestPointVisualiser.visualiseInterestPoints(outImg,
                                adapt.points);
                DisplayUtilities.display(ipv.drawPatches(RGBColour.BLUE, RGBColour.RED));

        }

        /**
         * @param b
         *            whether the differencial scaling should be done iteratively
         *            (slow) or not (fast)
         */
        public void setFastDifferentiationScale(boolean b) {
                this.fastDifferentiationScale = b;
        }
}