/** @file
 * @brief Basic intersection routines
 *//*
 * Authors:
 *   Nathan Hurst <njh@njhurst.com>
 *   Marco Cecchetti <mrcekets at gmail.com>
 *   Jean-François Barraud <jf.barraud@gmail.com>
 * 
 * Copyright 2008-2009 Authors
 *
 * This library is free software; you can redistribute it and/or
 * modify it either under the terms of the GNU Lesser General Public
 * License version 2.1 as published by the Free Software Foundation
 * (the "LGPL") or, at your option, under the terms of the Mozilla
 * Public License Version 1.1 (the "MPL"). If you do not alter this
 * notice, a recipient may use your version of this file under either
 * the MPL or the LGPL.
 *
 * You should have received a copy of the LGPL along with this library
 * in the file COPYING-LGPL-2.1; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 * You should have received a copy of the MPL along with this library
 * in the file COPYING-MPL-1.1
 *
 * The contents of this file are subject to the Mozilla Public License
 * Version 1.1 (the "License"); you may not use this file except in
 * compliance with the License. You may obtain a copy of the License at
 * http://www.mozilla.org/MPL/
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
 * OF ANY KIND, either express or implied. See the LGPL or the MPL for
 * the specific language governing rights and limitations.
 *
 */

#include <2geom/basic-intersection.h>
#include <2geom/sbasis-to-bezier.h>
#include <2geom/exception.h>

#ifdef HAVE_GSL
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#endif

using std::vector;
namespace Geom {

//#ifdef USE_RECURSIVE_INTERSECTOR

// void find_intersections(std::vector<std::pair<double, double> > &xs,
//                         D2<SBasis> const & A,
//                         D2<SBasis> const & B) {
//     vector<Point> BezA, BezB;
//     sbasis_to_bezier(BezA, A);
//     sbasis_to_bezier(BezB, B);
    
//     xs.clear();
    
//     find_intersections_bezier_recursive(xs, BezA, BezB);
// }
// void find_intersections(std::vector< std::pair<double, double> > & xs,
//                          std::vector<Point> const& A,
//                          std::vector<Point> const& B,
//                         double precision){
//     find_intersections_bezier_recursive(xs, A, B, precision);
// }

//#else

namespace detail{ namespace bezier_clipping {
void portion(std::vector<Point> &B, Interval const &I);
void derivative(std::vector<Point> &D, std::vector<Point> const &B);
}; };

void find_intersections(std::vector<std::pair<double, double> > &xs,
                        D2<Bezier> const & A,
                        D2<Bezier> const & B,
                        double precision)
{
    find_intersections_bezier_clipping(xs, bezier_points(A), bezier_points(B), precision);
}

void find_intersections(std::vector<std::pair<double, double> > &xs,
                        D2<SBasis> const & A,
                        D2<SBasis> const & B,
                        double precision)
{
    vector<Point> BezA, BezB;
    sbasis_to_bezier(BezA, A);
    sbasis_to_bezier(BezB, B);
    
    find_intersections_bezier_clipping(xs, BezA, BezB, precision);
}

void find_intersections(std::vector< std::pair<double, double> > & xs,
                        std::vector<Point> const& A,
                        std::vector<Point> const& B,
                        double precision)
{
    find_intersections_bezier_clipping(xs, A, B, precision);
}

//#endif

/*
 * split the curve at the midpoint, returning an array with the two parts
 * Temporary storage is minimized by using part of the storage for the result
 * to hold an intermediate value until it is no longer needed.
 */
// TODO replace with Bezier method
void split(vector<Point> const &p, double t, 
           vector<Point> &left, vector<Point> &right) {
    const unsigned sz = p.size();
    //Geom::Point Vtemp[sz][sz];
    vector<vector<Point> > Vtemp(sz);
    for ( size_t i = 0; i < sz; ++i )
        Vtemp[i].reserve(sz);

    /* Copy control points	*/
    std::copy(p.begin(), p.end(), Vtemp[0].begin());

    /* Triangle computation	*/
    for (unsigned i = 1; i < sz; i++) {
        for (unsigned j = 0; j < sz - i; j++) {
            Vtemp[i][j] = lerp(t, Vtemp[i-1][j], Vtemp[i-1][j+1]);
        }
    }

    left.resize(sz);
    right.resize(sz);
    for (unsigned j = 0; j < sz; j++)
        left[j]  = Vtemp[j][0];
    for (unsigned j = 0; j < sz; j++)
        right[j] = Vtemp[sz-1-j][j];
}



void find_self_intersections(std::vector<std::pair<double, double> > &xs,
                             D2<Bezier> const &A,
                             double precision)
{
    std::vector<double> dr = derivative(A[X]).roots();
    {
        std::vector<double> dyr = derivative(A[Y]).roots();
        dr.insert(dr.begin(), dyr.begin(), dyr.end());
    }
    dr.push_back(0);
    dr.push_back(1);
    // We want to be sure that we have no empty segments
    std::sort(dr.begin(), dr.end());
    std::vector<double>::iterator new_end = std::unique(dr.begin(), dr.end());
    dr.resize( new_end - dr.begin() );

    std::vector< D2<Bezier> > pieces;
    for (unsigned i = 0; i < dr.size() - 1; ++i) {
        pieces.push_back(portion(A, dr[i], dr[i+1]));
    }
    /*{
        vector<Point> l, r, in = A;
        for(unsigned i = 0; i < dr.size()-1; i++) {
            split(in, (dr[i+1]-dr[i]) / (1 - dr[i]), l, r);
            pieces.push_back(l);
            in = r;
        }
    }*/

    for(unsigned i = 0; i < dr.size()-1; i++) {
        for(unsigned j = i+1; j < dr.size()-1; j++) {
            std::vector<std::pair<double, double> > section;
            
            find_intersections(section, pieces[i], pieces[j], precision);
            for(unsigned k = 0; k < section.size(); k++) {
                double l = section[k].first;
                double r = section[k].second;
// XXX: This condition will prune out false positives, but it might create some false negatives.  Todo: Confirm it is correct.
                if(j == i+1)
                    //if((l == 1) && (r == 0))
                    if( ( l > precision ) && (r < precision) )//FIXME: what precision should be used here???
                        continue;
                xs.push_back(std::make_pair((1-l)*dr[i] + l*dr[i+1],
                                                (1-r)*dr[j] + r*dr[j+1]));
            }
        }
    }

    // Because i is in order, xs should be roughly already in order?
    //sort(xs.begin(), xs.end());
    //unique(xs.begin(), xs.end());
}

void find_self_intersections(std::vector<std::pair<double, double> > &xs,
                             D2<SBasis> const &A,
                             double precision)
{
    D2<Bezier> in;
    sbasis_to_bezier(in, A);
    find_self_intersections(xs, in, precision);
}


void subdivide(D2<Bezier> const &a,
               D2<Bezier> const &b,
               std::vector< std::pair<double, double> > const &xs,
               std::vector< D2<Bezier> > &av,
               std::vector< D2<Bezier> > &bv)
{
    if (xs.empty()) {
        av.push_back(a);
        bv.push_back(b);
        return;
    }

    std::pair<double, double> prev = std::make_pair(0., 0.);
    for (unsigned i = 0; i < xs.size(); ++i) {
        av.push_back(portion(a, prev.first, xs[i].first));
        bv.push_back(portion(b, prev.second, xs[i].second));
        av.back()[X].at0() = bv.back()[X].at0() = lerp(0.5, av.back()[X].at0(), bv.back()[X].at0());
        av.back()[X].at1() = bv.back()[X].at1() = lerp(0.5, av.back()[X].at1(), bv.back()[X].at1());
        av.back()[Y].at0() = bv.back()[Y].at0() = lerp(0.5, av.back()[Y].at0(), bv.back()[Y].at0());
        av.back()[Y].at1() = bv.back()[Y].at1() = lerp(0.5, av.back()[Y].at1(), bv.back()[Y].at1());
        prev = xs[i];
    }
    av.push_back(portion(a, prev.first, 1));
    bv.push_back(portion(b, prev.second, 1));
    av.back()[X].at0() = bv.back()[X].at0() = lerp(0.5, av.back()[X].at0(), bv.back()[X].at0());
    av.back()[X].at1() = bv.back()[X].at1() = lerp(0.5, av.back()[X].at1(), bv.back()[X].at1());
    av.back()[Y].at0() = bv.back()[Y].at0() = lerp(0.5, av.back()[Y].at0(), bv.back()[Y].at0());
    av.back()[Y].at1() = bv.back()[Y].at1() = lerp(0.5, av.back()[Y].at1(), bv.back()[Y].at1());
}

#ifdef HAVE_GSL
#include <gsl/gsl_multiroots.h>

struct rparams
{
    D2<SBasis> const &A;
    D2<SBasis> const &B;
};

static int
intersect_polish_f (const gsl_vector * x, void *params,
                    gsl_vector * f)
{
    const double x0 = gsl_vector_get (x, 0);
    const double x1 = gsl_vector_get (x, 1);

    Geom::Point dx = ((struct rparams *) params)->A(x0) -
        ((struct rparams *) params)->B(x1);

    gsl_vector_set (f, 0, dx[0]);
    gsl_vector_set (f, 1, dx[1]);

    return GSL_SUCCESS;
}
#endif

union dbl_64{
    long long i64;
    double d64;
};

static double EpsilonBy(double value, int eps)
{
    dbl_64 s;
    s.d64 = value;
    s.i64 += eps;
    return s.d64;
}


static void intersect_polish_root (D2<SBasis> const &A, double &s,
                                   D2<SBasis> const &B, double &t) {
#ifdef HAVE_GSL
    const gsl_multiroot_fsolver_type *T;
    gsl_multiroot_fsolver *sol;

    int status;
    size_t iter = 0;
#endif
    std::vector<Point> as, bs;
    as = A.valueAndDerivatives(s, 2);
    bs = B.valueAndDerivatives(t, 2);
    Point F = as[0] - bs[0];
    double best = dot(F, F);
    
    for(int i = 0; i < 4; i++) {
        
        /**
           we want to solve
           J*(x1 - x0) = f(x0)
           
           |dA(s)[0]  -dB(t)[0]|  (X1 - X0) = A(s) - B(t)
           |dA(s)[1]  -dB(t)[1]| 
        **/

        // We're using the standard transformation matricies, which is numerically rather poor.  Much better to solve the equation using elimination.

        Affine jack(as[1][0], as[1][1],
                    -bs[1][0], -bs[1][1],
                    0, 0);
        Point soln = (F)*jack.inverse();
        double ns = s - soln[0];
        double nt = t - soln[1];
        
        as = A.valueAndDerivatives(ns, 2);
        bs = B.valueAndDerivatives(nt, 2);
        F = as[0] - bs[0];
        double trial = dot(F, F);
        if (trial > best*0.1) {// we have standards, you know
            // At this point we could do a line search
            break;
        }
        best = trial;
        s = ns;
        t = nt;
    }
    
#ifdef HAVE_GSL
    const size_t n = 2;
    struct rparams p = {A, B};
    gsl_multiroot_function f = {&intersect_polish_f, n, &p};

    double x_init[2] = {s, t};
    gsl_vector *x = gsl_vector_alloc (n);

    gsl_vector_set (x, 0, x_init[0]);
    gsl_vector_set (x, 1, x_init[1]);

    T = gsl_multiroot_fsolver_hybrids;
    sol = gsl_multiroot_fsolver_alloc (T, 2);
    gsl_multiroot_fsolver_set (sol, &f, x);

    do
    {
        iter++;
        status = gsl_multiroot_fsolver_iterate (sol);

        if (status)   /* check if solver is stuck */
            break;

        status =
            gsl_multiroot_test_residual (sol->f, 1e-12);
    }
    while (status == GSL_CONTINUE && iter < 1000);

    s = gsl_vector_get (sol->x, 0);
    t = gsl_vector_get (sol->x, 1);

    gsl_multiroot_fsolver_free (sol);
    gsl_vector_free (x);
#endif
    
    {
    // This code does a neighbourhood search for minor improvements.
    double best_v = L1(A(s) - B(t));
    //std::cout  << "------\n" <<  best_v << std::endl;
    Point best(s,t);
    while (true) {
        Point trial = best;
        double trial_v = best_v;
        for(int nsi = -1; nsi < 2; nsi++) {
        for(int nti = -1; nti < 2; nti++) {
            Point n(EpsilonBy(best[0], nsi),
                    EpsilonBy(best[1], nti));
            double c = L1(A(n[0]) - B(n[1]));
            //std::cout << c << "; ";
            if (c < trial_v) {
                trial = n;
                trial_v = c;
            }
        }
        }
        if(trial == best) {
            //std::cout << "\n" << s << " -> " << s - best[0] << std::endl;
            //std::cout << t << " -> " << t - best[1] << std::endl;
            //std::cout << best_v << std::endl;
            s = best[0];
            t = best[1];
            return;
        } else {
            best = trial;
            best_v = trial_v;
        }
    }
    }
}


void polish_intersections(std::vector<std::pair<double, double> > &xs, 
                        D2<SBasis> const  &A, D2<SBasis> const &B)
{
    for(unsigned i = 0; i < xs.size(); i++)
        intersect_polish_root(A, xs[i].first,
                              B, xs[i].second);
}

/**
 * Compute the Hausdorf distance from A to B only.
 */
double hausdorfl(D2<SBasis>& A, D2<SBasis> const& B,
                 double m_precision,
                 double *a_t, double* b_t) {
    std::vector< std::pair<double, double> > xs;
    std::vector<Point> Az, Bz;
    sbasis_to_bezier (Az, A);
    sbasis_to_bezier (Bz, B);
    find_collinear_normal(xs, Az, Bz, m_precision);
    double h_dist = 0, h_a_t = 0, h_b_t = 0;
    double dist = 0;
    Point Ax = A.at0();
    double t = Geom::nearest_time(Ax, B);
    dist = Geom::distance(Ax, B(t));
    if (dist > h_dist) {
        h_a_t = 0;
        h_b_t = t;
        h_dist = dist;
    }
    Ax = A.at1();
    t = Geom::nearest_time(Ax, B);
    dist = Geom::distance(Ax, B(t));
    if (dist > h_dist) {
        h_a_t = 1;
        h_b_t = t;
        h_dist = dist;
    }
    for (size_t i = 0; i < xs.size(); ++i)
    {
        Point At = A(xs[i].first);
        Point Bu = B(xs[i].second);
        double distAtBu = Geom::distance(At, Bu);
        t = Geom::nearest_time(At, B);
        dist = Geom::distance(At, B(t));
        //FIXME: we might miss it due to floating point precision...
        if (dist >= distAtBu-.1 && distAtBu > h_dist) {
            h_a_t = xs[i].first;
            h_b_t = xs[i].second;
            h_dist = distAtBu;
        }
            
    }
    if(a_t) *a_t = h_a_t;
    if(b_t) *b_t = h_b_t;
    
    return h_dist;
}

/** 
 * Compute the symmetric Hausdorf distance.
 */
double hausdorf(D2<SBasis>& A, D2<SBasis> const& B,
                 double m_precision,
                 double *a_t, double* b_t) {
    double h_dist = hausdorfl(A, B, m_precision, a_t, b_t);
    
    double dist = 0;
    Point Bx = B.at0();
    double t = Geom::nearest_time(Bx, A);
    dist = Geom::distance(Bx, A(t));
    if (dist > h_dist) {
        if(a_t) *a_t = t;
        if(b_t) *b_t = 0;
        h_dist = dist;
    }
    Bx = B.at1();
    t = Geom::nearest_time(Bx, A);
    dist = Geom::distance(Bx, A(t));
    if (dist > h_dist) {
        if(a_t) *a_t = t;
        if(b_t) *b_t = 1;
        h_dist = dist;
    }
    
    return h_dist;
}

bool non_collinear_segments_intersect(const Point &A, const Point &B, const Point &C, const Point &D)
{
    return cross(D - C, A - C) * cross(D - C, B - C) < 0 && //
           cross(B - A, C - A) * cross(B - A, D - A) < 0;
}
};

/*
  Local Variables:
  mode:c++
  c-file-style:"stroustrup"
  c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
  indent-tabs-mode:nil
  fill-column:99
  End:
*/
// vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:fileencoding=utf-8:textwidth=99 :