/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
 * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
 *************************************************************************/

#ifndef OPENCV_FLANN_SIMPLEX_DOWNHILL_H_
#define OPENCV_FLANN_SIMPLEX_DOWNHILL_H_

namespace cvflann
{

/**
    Adds val to array vals (and point to array points) and keeping the arrays sorted by vals.
 */
template <typename T>
void addValue(int pos, float val, float* vals, T* point, T* points, int n)
{
    vals[pos] = val;
    for (int i=0; i<n; ++i) {
        points[pos*n+i] = point[i];
    }

    // bubble down
    int j=pos;
    while (j>0 && vals[j]<vals[j-1]) {
        swap(vals[j],vals[j-1]);
        for (int i=0; i<n; ++i) {
            swap(points[j*n+i],points[(j-1)*n+i]);
        }
        --j;
    }
}


/**
    Simplex downhill optimization function.
    Preconditions: points is a 2D mattrix of size (n+1) x n
                    func is the cost function taking n an array of n params and returning float
                    vals is the cost function in the n+1 simplex points, if NULL it will be computed

    Postcondition: returns optimum value and points[0..n] are the optimum parameters
 */
template <typename T, typename F>
float optimizeSimplexDownhill(T* points, int n, F func, float* vals = NULL )
{
    const int MAX_ITERATIONS = 10;

    assert(n>0);

    T* p_o = new T[n];
    T* p_r = new T[n];
    T* p_e = new T[n];

    int alpha = 1;

    int iterations = 0;

    bool ownVals = false;
    if (vals == NULL) {
        ownVals = true;
        vals = new float[n+1];
        for (int i=0; i<n+1; ++i) {
            float val = func(points+i*n);
            addValue(i, val, vals, points+i*n, points, n);
        }
    }
    int nn = n*n;

    while (true) {

        if (iterations++ > MAX_ITERATIONS) break;

        // compute average of simplex points (except the highest point)
        for (int j=0; j<n; ++j) {
            p_o[j] = 0;
            for (int i=0; i<n; ++i) {
                p_o[i] += points[j*n+i];
            }
        }
        for (int i=0; i<n; ++i) {
            p_o[i] /= n;
        }

        bool converged = true;
        for (int i=0; i<n; ++i) {
            if (p_o[i] != points[nn+i]) {
                converged = false;
            }
        }
        if (converged) break;

        // trying a reflection
        for (int i=0; i<n; ++i) {
            p_r[i] = p_o[i] + alpha*(p_o[i]-points[nn+i]);
        }
        float val_r = func(p_r);

        if ((val_r>=vals[0])&&(val_r<vals[n])) {
            // reflection between second highest and lowest
            // add it to the simplex
            Logger::info("Choosing reflection\n");
            addValue(n, val_r,vals, p_r, points, n);
            continue;
        }

        if (val_r<vals[0]) {
            // value is smaller than smalest in simplex

            // expand some more to see if it drops further
            for (int i=0; i<n; ++i) {
                p_e[i] = 2*p_r[i]-p_o[i];
            }
            float val_e = func(p_e);

            if (val_e<val_r) {
                Logger::info("Choosing reflection and expansion\n");
                addValue(n, val_e,vals,p_e,points,n);
            }
            else {
                Logger::info("Choosing reflection\n");
                addValue(n, val_r,vals,p_r,points,n);
            }
            continue;
        }
        if (val_r>=vals[n]) {
            for (int i=0; i<n; ++i) {
                p_e[i] = (p_o[i]+points[nn+i])/2;
            }
            float val_e = func(p_e);

            if (val_e<vals[n]) {
                Logger::info("Choosing contraction\n");
                addValue(n,val_e,vals,p_e,points,n);
                continue;
            }
        }
        {
            Logger::info("Full contraction\n");
            for (int j=1; j<=n; ++j) {
                for (int i=0; i<n; ++i) {
                    points[j*n+i] = (points[j*n+i]+points[i])/2;
                }
                float val = func(points+j*n);
                addValue(j,val,vals,points+j*n,points,n);
            }
        }
    }

    float bestVal = vals[0];

    delete[] p_r;
    delete[] p_o;
    delete[] p_e;
    if (ownVals) delete[] vals;

    return bestVal;
}

}

#endif //OPENCV_FLANN_SIMPLEX_DOWNHILL_H_