/** @file
 * @brief Nearest time routines for D2<SBasis> and Piecewise<D2<SBasis>>
 *//*
 * Authors:
 *   Marco Cecchetti <mrcekets at gmail.com>
 *
 * Copyright 2007-2008  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/nearest-time.h>
#include <algorithm>

namespace Geom
{

Coord nearest_time(Point const &p, D2<Bezier> const &input, Coord from, Coord to)
{
    Interval domain(from, to);
    bool partial = false;

    if (domain.min() < 0 || domain.max() > 1) {
        THROW_RANGEERROR("[from,to] interval out of bounds");
    }

    if (input.isConstant(0)) return from;

    D2<Bezier> bez;
    if (domain.min() != 0 || domain.max() != 1) {
        bez = portion(input, domain) - p;
        partial = true;
    } else {
        bez = input - p;
    }

    // find extrema of the function x(t)^2 + y(t)^2
    // use the fact that (f^2)' = 2 f f'
    // this reduces the order of the distance function by 1
    D2<Bezier> deriv = derivative(bez);
    std::vector<Coord> ts = (multiply(bez[X], deriv[X]) + multiply(bez[Y], deriv[Y])).roots();

    Coord t = -1, mind = infinity();
    for (unsigned i = 0; i < ts.size(); ++i) {
        Coord droot = L2sq(bez.valueAt(ts[i]));
        if (droot < mind) {
            mind = droot;
            t = ts[i];
        }
    }

    // also check endpoints
    Coord dinitial = L2sq(bez.at0());
    Coord dfinal = L2sq(bez.at1());

    if (dinitial < mind) {
        mind = dinitial;
        t = 0;
    }
    if (dfinal < mind) {
        //mind = dfinal;
        t = 1;
    }

    if (partial) {
        t = domain.valueAt(t);
    }
    return t;
}

////////////////////////////////////////////////////////////////////////////////
// D2<SBasis> versions

/*
 * Return the parameter t of the nearest time value on the portion of the curve "c",
 * related to the interval [from, to], to the point "p".
 * The needed curve derivative "dc" is passed as parameter.
 * The function return the first nearest time value to "p" that is found.
 */

double nearest_time(Point const& p,
                    D2<SBasis> const& c,
                    D2<SBasis> const& dc,
                    double from, double to )
{
    if ( from > to ) std::swap(from, to);
    if ( from < 0 || to > 1 )
    {
        THROW_RANGEERROR("[from,to] interval out of bounds");
    }
    if (c.isConstant()) return from;
    SBasis dd = dot(c - p, dc);
    //std::cout << dd << std::endl;
    std::vector<double> zeros = Geom::roots(dd);

    double closest = from;
    double min_dist_sq = L2sq(c(from) - p);
    for ( size_t i = 0; i < zeros.size(); ++i )
    {
        double distsq = L2sq(c(zeros[i]) - p);
        if ( min_dist_sq > L2sq(c(zeros[i]) - p) )
        {
            closest = zeros[i];
            min_dist_sq = distsq;
        }
    }
    if ( min_dist_sq > L2sq( c(to) - p ) )
        closest = to;
    return closest;

}

/*
 * Return the parameters t of all the nearest points on the portion of
 * the curve "c", related to the interval [from, to], to the point "p".
 * The needed curve derivative "dc" is passed as parameter.
 */

std::vector<double>
all_nearest_times(Point const &p,
                  D2<SBasis> const &c,
                  D2<SBasis> const &dc,
                  double from, double to)
{
    if (from > to) {
        std::swap(from, to);
    }
    if (from < 0 || to > 1) {
        THROW_RANGEERROR("[from,to] interval out of bounds");
    }

    std::vector<double> result;
    if (c.isConstant()) {
        result.push_back(from);
        return result;
    }
    SBasis dd = dot(c - p, dc);

    std::vector<double> zeros = Geom::roots(dd);
    std::vector<double> candidates;
    candidates.push_back(from);
    candidates.insert(candidates.end(), zeros.begin(), zeros.end());
    candidates.push_back(to);
    std::vector<double> distsq;
    distsq.reserve(candidates.size());
    for (unsigned i = 0; i < candidates.size(); ++i) {
        distsq.push_back(L2sq(c(candidates[i]) - p));
    }
    unsigned closest = 0;
    double dsq = distsq[0];
    for (unsigned i = 1; i < candidates.size(); ++i) {
        if (dsq > distsq[i]) {
            closest = i;
            dsq = distsq[i];
        }
    }
    for (unsigned i = 0; i < candidates.size(); ++i) {
        if (distsq[closest] == distsq[i]) {
            result.push_back(candidates[i]);
        }
    }
    return result;
}


////////////////////////////////////////////////////////////////////////////////
// Piecewise< D2<SBasis> > versions


double nearest_time(Point const &p,
                    Piecewise< D2<SBasis> > const &c,
                    double from, double to)
{
    if (from > to) std::swap(from, to);
    if (from < c.cuts[0] || to > c.cuts[c.size()]) {
        THROW_RANGEERROR("[from,to] interval out of bounds");
    }

    unsigned si = c.segN(from);
    unsigned ei = c.segN(to);
    if (si == ei) {
        double nearest =
            nearest_time(p, c[si], c.segT(from, si), c.segT(to, si));
        return c.mapToDomain(nearest, si);
    }

    double t;
    double nearest = nearest_time(p, c[si], c.segT(from, si));
    unsigned int ni = si;
    double dsq;
    double mindistsq = distanceSq(p, c[si](nearest));
    Rect bb;
    for (unsigned i = si + 1; i < ei; ++i) {
        bb = *bounds_fast(c[i]);
        dsq = distanceSq(p, bb);
        if ( mindistsq <= dsq ) continue;

        t = nearest_time(p, c[i]);
        dsq = distanceSq(p, c[i](t));
        if (mindistsq > dsq) {
            nearest = t;
            ni = i;
            mindistsq = dsq;
        }
    }
    bb = *bounds_fast(c[ei]);
    dsq = distanceSq(p, bb);
    if (mindistsq > dsq) {
        t = nearest_time(p, c[ei], 0, c.segT(to, ei));
        dsq = distanceSq(p, c[ei](t));
        if (mindistsq > dsq) {
            nearest = t;
            ni = ei;
        }
    }
    return c.mapToDomain(nearest, ni);
}

std::vector<double>
all_nearest_times(Point const &p,
                  Piecewise< D2<SBasis> > const &c,
                  double from, double to)
{
    if (from > to) {
        std::swap(from, to);
    }
    if (from < c.cuts[0] || to > c.cuts[c.size()]) {
        THROW_RANGEERROR("[from,to] interval out of bounds");
    }

    unsigned si = c.segN(from);
    unsigned ei = c.segN(to);
    if ( si == ei )
    {
        std::vector<double>	all_nearest =
            all_nearest_times(p, c[si], c.segT(from, si), c.segT(to, si));
        for ( unsigned int i = 0; i < all_nearest.size(); ++i )
        {
            all_nearest[i] = c.mapToDomain(all_nearest[i], si);
        }
        return all_nearest;
    }
    std::vector<double> all_t;
    std::vector< std::vector<double> > all_np;
    all_np.push_back( all_nearest_times(p, c[si], c.segT(from, si)) );
    std::vector<unsigned> ni;
    ni.push_back(si);
    double dsq;
    double mindistsq = distanceSq( p, c[si](all_np.front().front()) );
    Rect bb;

    for (unsigned i = si + 1; i < ei; ++i) {
        bb = *bounds_fast(c[i]);
        dsq = distanceSq(p, bb);
        if ( mindistsq < dsq ) continue;
        all_t = all_nearest_times(p, c[i]);
        dsq = distanceSq( p, c[i](all_t.front()) );
        if ( mindistsq > dsq )
        {
            all_np.clear();
            all_np.push_back(all_t);
            ni.clear();
            ni.push_back(i);
            mindistsq = dsq;
        }
        else if ( mindistsq == dsq )
        {
            all_np.push_back(all_t);
            ni.push_back(i);
        }
    }
    bb = *bounds_fast(c[ei]);
    dsq = distanceSq(p, bb);
    if (mindistsq >= dsq) {
        all_t = all_nearest_times(p, c[ei], 0, c.segT(to, ei));
        dsq = distanceSq( p, c[ei](all_t.front()) );
        if (mindistsq > dsq) {
            for (unsigned int i = 0; i < all_t.size(); ++i) {
                all_t[i] = c.mapToDomain(all_t[i], ei);
            }
            return all_t;
        } else if (mindistsq == dsq) {
            all_np.push_back(all_t);
            ni.push_back(ei);
        }
    }
    std::vector<double> all_nearest;
    for (unsigned i = 0; i < all_np.size(); ++i) {
        for (unsigned int j = 0; j < all_np[i].size(); ++j) {
            all_nearest.push_back( c.mapToDomain(all_np[i][j], ni[i]) );
        }
    }
    all_nearest.erase(std::unique(all_nearest.begin(), all_nearest.end()),
                      all_nearest.end());
    return all_nearest;
}

} // end namespace Geom