/*
    Scan Tailor - Interactive post-processing tool for scanned pages.
    Copyright (C) 2015  Joseph Artsimovich <joseph.artsimovich@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "XSpline.h"
#include "VecNT.h"
#include "NumericTraits.h"
#include "ToLineProjector.h"
#include "adiff/SparseMap.h"
#include "adiff/Function.h"
#include <QLineF>
#include <QDebug>
#include <boost/foreach.hpp>
#include <string>
#include <stdexcept>
#include <algorithm>
#include <math.h>
#include <assert.h>

struct XSpline::TensionDerivedParams
{
	static double const t0;
	static double const t1;
	static double const t2;
	static double const t3;

	// These correspond to Tk- and Tk+ in [18].
	double T0p;
	double T1p;
	double T2m;
	double T3m;

	// q parameters for GBlendFunc and HBlendFunc.
	double q[4];

	// p parameters for GBlendFunc.
	double p[4];

	TensionDerivedParams(double tension1, double tension2);
};


class XSpline::GBlendFunc
{
public:
	GBlendFunc(double q, double p);

	double value(double u) const;

	double firstDerivative(double u) const;

	double secondDerivative(double u) const;
private:
	double m_c1;
	double m_c2;
	double m_c3;
	double m_c4;
	double m_c5;
};


class XSpline::HBlendFunc
{
public:
	HBlendFunc(double q);

	double value(double u) const;

	double firstDerivative(double u) const;

	double secondDerivative(double u) const;
private:
	double m_c1;
	double m_c2;
	double m_c4;
	double m_c5;
};


struct XSpline::DecomposedDerivs
{
	double zeroDerivCoeffs[4];
	double firstDerivCoeffs[4];
	double secondDerivCoeffs[4];
	int controlPoints[4];
	int numControlPoints;

	bool hasNonZeroCoeffs(int idx) const {
		double sum = fabs(zeroDerivCoeffs[idx]) + fabs(firstDerivCoeffs[idx]) + fabs(secondDerivCoeffs[idx]);
		return sum > std::numeric_limits<double>::epsilon();
	}
};


int
XSpline::numControlPoints() const
{
	return m_controlPoints.size();
}

int
XSpline::numSegments() const
{
	return std::max<int>(m_controlPoints.size() - 1, 0);
}

double
XSpline::controlPointIndexToT(int idx) const
{
	assert(idx >= 0 || idx <= (int)m_controlPoints.size());
	return double(idx) / (m_controlPoints.size() - 1);
}

void
XSpline::appendControlPoint(QPointF const& pos, double tension)
{
	m_controlPoints.push_back(ControlPoint(pos, tension));
}

void
XSpline::insertControlPoint(int idx, QPointF const& pos, double tension)
{
	assert(idx >= 0 || idx <= (int)m_controlPoints.size());
	m_controlPoints.insert(m_controlPoints.begin() + idx, ControlPoint(pos, tension));
}

void
XSpline::eraseControlPoint(int idx)
{
	assert(idx >= 0 || idx < (int)m_controlPoints.size());
	m_controlPoints.erase(m_controlPoints.begin() + idx);
}

QPointF
XSpline::controlPointPosition(int idx) const
{
	return m_controlPoints[idx].pos;
}

void
XSpline::moveControlPoint(int idx, QPointF const& pos)
{
	assert(idx >= 0 || idx < (int)m_controlPoints.size());
	m_controlPoints[idx].pos = pos;
}

double
XSpline::controlPointTension(int idx) const
{
	assert(idx >= 0 && idx < (int)m_controlPoints.size());
	return m_controlPoints[idx].tension;
}

void
XSpline::setControlPointTension(int idx, double tension)
{
	assert(idx >= 0 && idx < (int)m_controlPoints.size());
	m_controlPoints[idx].tension = tension;
}

QPointF
XSpline::pointAt(double t) const
{
	int const num_segments = numSegments();
	assert(num_segments > 0);
	assert(t >= 0 && t <= 1);

	if (t == 1.0) {
		// If we went with the branch below, we would end up with
		// segment == num_segments, which is an error.
		return pointAtImpl(num_segments - 1, 1.0);
	} else {
		double const t2 = t * num_segments;
		double const segment = floor(t2);
		return pointAtImpl((int)segment, t2 - segment);
	}
}

QPointF
XSpline::pointAtImpl(int segment, double t) const
{
	LinearCoefficient coeffs[4];
	int const num_coeffs = linearCombinationFor(coeffs, segment, t);

	QPointF pt(0, 0);
	for (int i = 0; i < num_coeffs; ++i) {
		LinearCoefficient const& c = coeffs[i];
		pt += m_controlPoints[c.controlPointIdx].pos * c.coeff;
	}

	return pt;
}

void
XSpline::sample(
	VirtualFunction3<void, QPointF, double, SampleFlags>& sink,
	SamplingParams const& params, double from_t, double to_t) const
{
	if (m_controlPoints.empty()) {
		return;
	}

	double max_sqdist_to_spline = params.maxDistFromSpline;
	if (max_sqdist_to_spline != NumericTraits<double>::max()) {
		max_sqdist_to_spline *= params.maxDistFromSpline;
	}

	double max_sqdist_between_samples = params.maxDistBetweenSamples;
	if (max_sqdist_between_samples != NumericTraits<double>::max()) {
		max_sqdist_between_samples *= params.maxDistBetweenSamples;
	}

	QPointF const from_pt(pointAt(from_t));
	QPointF const to_pt(pointAt(to_t));
	sink(from_pt, from_t, HEAD_SAMPLE);
	
	int const num_segments = numSegments();
	if (num_segments == 0) {
		return;
	}
	double const r_num_segments = 1.0 / num_segments;

	maybeAddMoreSamples(
		sink, max_sqdist_to_spline, max_sqdist_between_samples,
		num_segments, r_num_segments, from_t, from_pt, to_t, to_pt
	);

	sink(to_pt, to_t, TAIL_SAMPLE);
}

void
XSpline::maybeAddMoreSamples(
	VirtualFunction3<void, QPointF, double, SampleFlags>& sink,
	double max_sqdist_to_spline, double max_sqdist_between_samples,
	double num_segments, double r_num_segments,
	double prev_t, QPointF const& prev_pt,
	double next_t, QPointF const& next_pt) const
{
	double const prev_next_sqdist = Vec2d(next_pt - prev_pt).squaredNorm();
	if (prev_next_sqdist < 1e-6) {
		// Too close. Projecting anything on such a small line segment is dangerous.
		return;
	}

	SampleFlags flags = DEFAULT_SAMPLE;
	double mid_t = 0.5 * (prev_t + next_t);
	double const nearby_junction_t = floor(mid_t * num_segments + 0.5) * r_num_segments;
	
	// If nearby_junction_t is between prev_t and next_t, make it our mid_t.
	if ((nearby_junction_t - prev_t) * (next_t - prev_t) > 0 &&
		(nearby_junction_t - next_t) * (prev_t - next_t) > 0) {
		mid_t = nearby_junction_t;
		flags = JUNCTION_SAMPLE;
	}

	QPointF const mid_pt(pointAt(mid_t));
	
	if (flags != JUNCTION_SAMPLE) {
		QPointF const projection(
			ToLineProjector(QLineF(prev_pt, next_pt)).projectionPoint(mid_pt)
		);

		if (prev_next_sqdist <= max_sqdist_between_samples) {
			if (Vec2d(mid_pt - projection).squaredNorm() <= max_sqdist_to_spline) {
				return;
			}
		}
	}

	maybeAddMoreSamples(
		sink, max_sqdist_to_spline, max_sqdist_between_samples,
		num_segments, r_num_segments, prev_t, prev_pt, mid_t, mid_pt
	);

	sink(mid_pt, mid_t, flags);
	
	maybeAddMoreSamples(
		sink, max_sqdist_to_spline, max_sqdist_between_samples,
		num_segments, r_num_segments, mid_t, mid_pt, next_t, next_pt
	);
}

void
XSpline::linearCombinationAt(double t, std::vector<LinearCoefficient>& coeffs) const
{
	assert(t >= 0 && t <= 1);

	int const num_segments = numSegments();
	assert(num_segments > 0);

	int num_coeffs = 4;
	coeffs.resize(num_coeffs);
	LinearCoefficient static_coeffs[4];

	if (t == 1.0) {
		// If we went with the branch below, we would end up with
		// segment == num_segments, which is an error.
		num_coeffs = linearCombinationFor(static_coeffs, num_segments - 1, 1.0);
	} else {
		double const t2 = t * num_segments;
		double const segment = floor(t2);
		num_coeffs = linearCombinationFor(static_coeffs, (int)segment, t2 - segment);
	}

	for (int i = 0; i < num_coeffs; ++i) {
		coeffs[i] = static_coeffs[i];
	}
	coeffs.resize(num_coeffs);
}

int
XSpline::linearCombinationFor(LinearCoefficient* coeffs, int segment, double t) const
{
	assert(segment >= 0 && segment < (int)m_controlPoints.size() - 1);
	assert(t >= 0 && t <= 1);

	int idxs[4];
	idxs[0] = std::max<int>(0, segment - 1);
	idxs[1] = segment;
	idxs[2] = segment + 1;
	idxs[3] = std::min<int>(segment + 2, m_controlPoints.size() - 1);

	ControlPoint pts[4];
	for (int i = 0; i < 4; ++i) {
		pts[i] = m_controlPoints[idxs[i]];
	}

	TensionDerivedParams const tdp(pts[1].tension, pts[2].tension);

	Vec4d A;

	if (t <= tdp.T0p) {
		A[0] = GBlendFunc(tdp.q[0], tdp.p[0]).value((t - tdp.T0p) / (tdp.t0 - tdp.T0p));
	} else {
		A[0] = HBlendFunc(tdp.q[0]).value((t - tdp.T0p) / (tdp.t0 - tdp.T0p));
	}

	A[1] = GBlendFunc(tdp.q[1], tdp.p[1]).value((t - tdp.T1p) / (tdp.t1 - tdp.T1p));
	A[2] = GBlendFunc(tdp.q[2], tdp.p[2]).value((t - tdp.T2m) / (tdp.t2 - tdp.T2m));

	if (t >= tdp.T3m) {
		A[3] = GBlendFunc(tdp.q[3], tdp.p[3]).value((t - tdp.T3m) / (tdp.t3 - tdp.T3m));
	} else {
		A[3] = HBlendFunc(tdp.q[3]).value((t - tdp.T3m) / (tdp.t3 - tdp.T3m));
	}

	A /= A.sum();

	int out_idx = 0;

	if (idxs[0] == idxs[1]) {
		coeffs[out_idx++] = LinearCoefficient(idxs[0], A[0] + A[1]);
	} else {
		coeffs[out_idx++] = LinearCoefficient(idxs[0], A[0]);
		coeffs[out_idx++] = LinearCoefficient(idxs[1], A[1]);
	}
	
	if (idxs[2] == idxs[3]) {
		coeffs[out_idx++] = LinearCoefficient(idxs[2], A[2] + A[3]);
	} else {
		coeffs[out_idx++] = LinearCoefficient(idxs[2], A[2]);
		coeffs[out_idx++] = LinearCoefficient(idxs[3], A[3]);
	}

	return out_idx;
}

XSpline::PointAndDerivs
XSpline::pointAndDtsAt(double t) const
{
	assert(t >= 0 && t <= 1);
	PointAndDerivs pd;

	DecomposedDerivs const derivs(decomposedDerivs(t));
	for (int i = 0; i < derivs.numControlPoints; ++i) {
		QPointF const& cp = m_controlPoints[derivs.controlPoints[i]].pos;
		pd.point += cp * derivs.zeroDerivCoeffs[i];
		pd.firstDeriv += cp * derivs.firstDerivCoeffs[i];
		pd.secondDeriv += cp * derivs.secondDerivCoeffs[i];
	}

	return pd;
}

XSpline::DecomposedDerivs
XSpline::decomposedDerivs(double const t) const
{
	assert(t >= 0 && t <= 1);

	int const num_segments = numSegments();
	assert(num_segments > 0);

	if (t == 1.0) {
		// If we went with the branch below, we would end up with
		// segment == num_segments, which is an error.
		return decomposedDerivsImpl(num_segments - 1, 1.0);
	} else {
		double const t2 = t * num_segments;
		double const segment = floor(t2);
		return decomposedDerivsImpl((int)segment, t2 - segment);
	}
}

XSpline::DecomposedDerivs
XSpline::decomposedDerivsImpl(int const segment, double const t) const
{
	assert(segment >= 0 && segment < (int)m_controlPoints.size() - 1);
	assert(t >= 0 && t <= 1);

	DecomposedDerivs derivs;

	derivs.numControlPoints = 4; // May be modified later in this function.
	derivs.controlPoints[0] = std::max<int>(0, segment - 1);
	derivs.controlPoints[1] = segment;
	derivs.controlPoints[2] = segment + 1;
	derivs.controlPoints[3] = std::min<int>(segment + 2, m_controlPoints.size() - 1);

	ControlPoint pts[4];
	for (int i = 0; i < 4; ++i) {
		pts[i] = m_controlPoints[derivs.controlPoints[i]];
	}

	TensionDerivedParams const tdp(pts[1].tension, pts[2].tension);

	// Note that we don't want the derivate with respect to t that's
	// passed to us (ranging from 0 to 1 within a segment).
	// Rather we want it with respect to the t that's passed to
	// decomposedDerivs(), ranging from 0 to 1 across all segments.
	// Let's call the latter capital T.  Their relationship is:
	// t = T*num_segments - C
	// dt/dT = num_segments
	double const dtdT = numSegments();

	Vec4d A;
	Vec4d dA; // First derivatives with respect to T.
	Vec4d ddA; // Second derivatives with respect to T.

	// Control point 0.
	{
		// u = (t - tdp.T0p) / (tdp.t0 - tdp.T0p)
		double const ta = 1.0 / (tdp.t0 - tdp.T0p);
		double const tb = -tdp.T0p * ta;
		double const u = ta * t + tb;		
		if (t <= tdp.T0p) {
			// u(t) = ta * tt + tb
			// u'(t) = ta
			// g(t) = g(u(t), <tension derived params>)
			GBlendFunc g(tdp.q[0], tdp.p[0]);
			A[0] = g.value(u);

			// g'(u(t(T))) = g'(u)*u'(t)*t'(T)
			dA[0] = g.firstDerivative(u) * (ta * dtdT);

			// Note that u'(t) and t'(T) are constant functions.
			// g"(u(t(T))) = g"(u)*u'(t)*t'(T)*u'(t)*t'(T)
			ddA[0] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
		} else {
			HBlendFunc h(tdp.q[0]);
			A[0] = h.value(u);
			dA[0] = h.firstDerivative(u) * (ta * dtdT);
			ddA[0] = h.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
		}
	}

	// Control point 1.
	{
		// u = (t - tdp.T1p) / (tdp.t1 - tdp.T1p)
		double const ta = 1.0 / (tdp.t1 - tdp.T1p);
		double const tb = -tdp.T1p * ta;
		double const u = ta * t + tb;
		GBlendFunc g(tdp.q[1], tdp.p[1]);
		A[1] = g.value(u);
		dA[1] = g.firstDerivative(u) * (ta * dtdT);
		ddA[1] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
	}

	// Control point 2.
	{
		// u = (t - tdp.T2m) / (tdp.t2 - tdp.T2m)
		double const ta = 1.0 / (tdp.t2 - tdp.T2m);
		double const tb = -tdp.T2m * ta;
		double const u = ta * t + tb;
		GBlendFunc g(tdp.q[2], tdp.p[2]);
		A[2] = g.value(u);
		dA[2] = g.firstDerivative(u) * (ta * dtdT);
		ddA[2] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
	}

	// Control point 3.
	{
		// u = (t - tdp.T3m) / (tdp.t3 - tdp.T3m)
		double const ta = 1.0 / (tdp.t3 - tdp.T3m);
		double const tb = -tdp.T3m * ta;
		double const u = ta * t + tb;
		if (t >= tdp.T3m) {
			GBlendFunc g(tdp.q[3], tdp.p[3]);
			A[3] = g.value(u);
			dA[3] = g.firstDerivative(u) * (ta * dtdT);
			ddA[3] = g.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
		} else {
			HBlendFunc h(tdp.q[3]);
			A[3] = h.value(u);
			dA[3] = h.firstDerivative(u) * (ta * dtdT);
			ddA[3] = h.secondDerivative(u) * (ta * dtdT) * (ta * dtdT);
		}
	}
	
	double const sum = A.sum();
	double const sum2 = sum * sum;
	double const sum4 = sum2 * sum2;
	double const d_sum = dA.sum();
	double const dd_sum = ddA.sum();

	for (int i = 0; i < 4; ++i) {
		derivs.zeroDerivCoeffs[i] = A[i] / sum;
 
		double const d1 = dA[i] * sum - A[i] * d_sum;
		derivs.firstDerivCoeffs[i] = d1 / sum2; // Derivative of: A[i] / sum
		
		// Derivative of: dA[i] * sum
		double const dd1 = ddA[i] * sum + dA[i] * d_sum;

		// Derivative of: A[i] * d_sum
		double const dd2 = dA[i] * d_sum + A[i] * dd_sum;

		// Derivative of (dA[i] * sum - A[i] * d_sum) / sum2
		double const dd3 = ((dd1 - dd2) * sum2 - d1 * (2 * sum * d_sum)) / sum4;
		derivs.secondDerivCoeffs[i] = dd3;
	}

	// Merge / throw away some control points.
	int write_idx = 0;
	int merge_idx = 0;
	int read_idx = 1;
	int const end = 4;
	for (;;) {
		assert(merge_idx != read_idx);
		for (; read_idx != end && derivs.controlPoints[read_idx] == derivs.controlPoints[merge_idx]; ++read_idx) {
			// Merge
			derivs.zeroDerivCoeffs[merge_idx] += derivs.zeroDerivCoeffs[read_idx];
			derivs.firstDerivCoeffs[merge_idx] += derivs.firstDerivCoeffs[read_idx];
			derivs.secondDerivCoeffs[merge_idx] += derivs.secondDerivCoeffs[read_idx];
		}

		if (derivs.hasNonZeroCoeffs(merge_idx)) {
			// Copy
			derivs.zeroDerivCoeffs[write_idx] = derivs.zeroDerivCoeffs[merge_idx];
			derivs.firstDerivCoeffs[write_idx] = derivs.firstDerivCoeffs[merge_idx];
			derivs.secondDerivCoeffs[write_idx] = derivs.secondDerivCoeffs[merge_idx];
			derivs.controlPoints[write_idx] = derivs.controlPoints[merge_idx];
			++write_idx;
		}
		
		if (read_idx == end) {
			break;
		}

		merge_idx = read_idx;
		++read_idx;
	}
	derivs.numControlPoints = write_idx;

	return derivs;
}

QuadraticFunction
XSpline::controlPointsAttractionForce() const
{
	return controlPointsAttractionForce(0, numSegments());
}

QuadraticFunction
XSpline::controlPointsAttractionForce(int seg_begin, int seg_end) const
{
	using namespace adiff;

	assert(seg_begin >= 0 && seg_begin <= numSegments());
	assert(seg_end >= 0 && seg_end <= numSegments());
	assert(seg_begin <= seg_end);

	int const num_control_points = numControlPoints();

	SparseMap<2> sparse_map(num_control_points * 2);
	sparse_map.markAllNonZero();

	Function<2> force(sparse_map);
	if (seg_begin != seg_end) {
		Function<2> prev_x(seg_begin * 2, m_controlPoints[seg_begin].pos.x(), sparse_map);
		Function<2> prev_y(seg_begin * 2 + 1, m_controlPoints[seg_begin].pos.y(), sparse_map);

		for (int i = seg_begin + 1; i <= seg_end; ++i) {
			Function<2> next_x(i * 2, m_controlPoints[i].pos.x(), sparse_map);
			Function<2> next_y(i * 2 + 1, m_controlPoints[i].pos.y(), sparse_map);

			Function<2> const dx(next_x - prev_x);
			Function<2> const dy(next_y - prev_y);
			force += dx * dx + dy * dy;

			next_x.swap(prev_x);
			next_y.swap(prev_y);
		}
	}

	QuadraticFunction f(num_control_points * 2);
	f.A = 0.5 * force.hessian(sparse_map);
	f.b = force.gradient(sparse_map);
	f.c = force.value;

	return f;
}

QuadraticFunction
XSpline::junctionPointsAttractionForce() const
{
	return junctionPointsAttractionForce(0, numSegments());
}

QuadraticFunction
XSpline::junctionPointsAttractionForce(int seg_begin, int seg_end) const
{
	using namespace adiff;

	assert(seg_begin >= 0 && seg_begin <= numSegments());
	assert(seg_end >= 0 && seg_end <= numSegments());
	assert(seg_begin <= seg_end);

	int const num_control_points = numControlPoints();

	SparseMap<2> sparse_map(num_control_points * 2);
	sparse_map.markAllNonZero();

	Function<2> force(sparse_map);

	if (seg_begin != seg_end) {
		QPointF pt(pointAt(controlPointIndexToT(seg_begin)));
		std::vector<LinearCoefficient> coeffs;
		Function<2> prev_x(0);
		Function<2> prev_y(0);

		for (int i = seg_begin; i <= seg_end; ++i) {
			Function<2> next_x(sparse_map);
			Function<2> next_y(sparse_map);
			
			linearCombinationAt(controlPointIndexToT(i), coeffs);
			BOOST_FOREACH(LinearCoefficient const& coeff, coeffs) {
				QPointF const cp(m_controlPoints[coeff.controlPointIdx].pos);
				Function<2> x(coeff.controlPointIdx * 2, cp.x(), sparse_map);
				Function<2> y(coeff.controlPointIdx * 2 + 1, cp.y(), sparse_map);
				x *= coeff.coeff;
				y *= coeff.coeff;
				next_x += x;
				next_y += y;
			}

			if (i != seg_begin) {
				Function<2> const dx(next_x - prev_x);
				Function<2> const dy(next_y - prev_y);
				force += dx * dx + dy * dy;
			}

			next_x.swap(prev_x);
			next_y.swap(prev_y);
		}
	}

	QuadraticFunction f(num_control_points * 2);
	f.A = 0.5 * force.hessian(sparse_map);
	f.b = force.gradient(sparse_map);
	f.c = force.value;

	return f;
}

QPointF
XSpline::pointClosestTo(QPointF const to, double* t, double accuracy) const
{
	if (m_controlPoints.empty()) {
		if (t) {
			*t = 0;
		}
		return QPointF();
	}

	int const num_segments = numSegments();
	if (num_segments == 0) {
		if (t) {
			*t = 0;
		}
		return m_controlPoints.front().pos;
	}

	QPointF prev_pt(pointAtImpl(0, 0));
	QPointF next_pt;
	
	// Find the closest segment.
	int best_segment = 0;
	double best_sqdist = Vec2d(to - prev_pt).squaredNorm();
	for (int seg = 0; seg < num_segments; ++seg, prev_pt = next_pt) {
		next_pt = pointAtImpl(seg, 1.0);

		double const sqdist = sqDistToLine(to, QLineF(prev_pt, next_pt));
		if (sqdist < best_sqdist) {
			best_segment = seg;
			best_sqdist = sqdist;
		}
	}

	// Continue with a binary search.
	double const sq_accuracy = accuracy * accuracy;
	double prev_t = 0;
	double next_t = 1;
	prev_pt = pointAtImpl(best_segment, prev_t);
	next_pt = pointAtImpl(best_segment, next_t);

	while (Vec2d(prev_pt - next_pt).squaredNorm() > sq_accuracy) {
		double const mid_t = 0.5 * (prev_t + next_t);
		QPointF const mid_pt(pointAtImpl(best_segment, mid_t));
		
		ToLineProjector const projector(QLineF(prev_pt, next_pt));
		double const pt = projector.projectionScalar(to);
		double const pm = projector.projectionScalar(mid_pt);
		if (pt < pm) {
			next_t = mid_t;
			next_pt = mid_pt;
		} else {
			prev_t = mid_t;
			prev_pt = mid_pt;
		}
	}

	// Take the closest of prev_pt and next_pt.
	if (Vec2d(to - prev_pt).squaredNorm() < Vec2d(to - next_pt).squaredNorm()) {
		if (t) {
			*t = (best_segment + prev_t) / num_segments;
		}
		return prev_pt;
	} else {
		if (t) {
			*t = (best_segment + next_t) / num_segments;
		}
		return next_pt;
	}
}

QPointF
XSpline::pointClosestTo(QPointF const to, double accuracy) const
{
	return pointClosestTo(to, 0, accuracy);
}

std::vector<QPointF>
XSpline::toPolyline(SamplingParams const& params, double from_t, double to_t) const
{
	struct Sink : public VirtualFunction3<void, QPointF, double, SampleFlags>
	{
		std::vector<QPointF> polyline;

		virtual void operator()(QPointF pt, double, SampleFlags) {
			polyline.push_back(pt);
		}
	};

	Sink sink;
	sample(sink, params, from_t, to_t);

	return sink.polyline;
}

double
XSpline::sqDistToLine(QPointF const& pt, QLineF const& line)
{
	ToLineProjector const projector(line);
	double const p = projector.projectionScalar(pt);
	QPointF ppt;
	if (p <= 0) {
		ppt = line.p1();
	} else if (p >= 1) {
		ppt = line.p2();
	} else {
		ppt = line.pointAt(p);
	}
	return Vec2d(pt - ppt).squaredNorm();
}


/*===================== TensionDerivedParams =====================*/

// We make t lie in [0 .. 1] within a segment,
// which gives us delta == 1 and the following tk's:
double const XSpline::TensionDerivedParams::t0 = -1;
double const XSpline::TensionDerivedParams::t1 = 0;
double const XSpline::TensionDerivedParams::t2 = 1;
double const XSpline::TensionDerivedParams::t3 = 2;

static double square(double v)
{
	return v*v;
}

XSpline::TensionDerivedParams::TensionDerivedParams(
	double const tension1, double const tension2)
{
	// tension1, tension2 lie in [-1 .. 1]

	// Tk+ = t(k+1) + s(k+1)
	// Tk- = t(k-1) - s(k-1)
	double const s1 = std::max<double>(tension1, 0);
	double const s2 = std::max<double>(tension2, 0);
	T0p = t1 + s1;
	T1p = t2 + s2;
	T2m = t1 - s1;
	T3m = t2 - s2;

	// q's lie in [0 .. 0.5]
	double const s_1 = -0.5 * std::min<double>(tension1, 0);
	double const s_2 = -0.5 * std::min<double>(tension2, 0);
	q[0] = s_1;
	q[1] = s_2;
	q[2] = s_1;
	q[3] = s_2;

	// Formula 17 in [18]:
	p[0] = 2.0 * square(t0 - T0p);
	p[1] = 2.0 * square(t1 - T1p);
	p[2] = 2.0 * square(t2 - T2m);
	p[3] = 2.0 * square(t3 - T3m);
}


/*========================== GBlendFunc ==========================*/

XSpline::GBlendFunc::GBlendFunc(double q, double p)
:	m_c1(q), // See formula 20 in [18].
	m_c2(2 * q),
	m_c3(10 - 12 * q - p),
	m_c4(2 * p + 14 * q - 15),
	m_c5(6 - 5 * q - p)
{
}

double
XSpline::GBlendFunc::value(double u) const
{
	double const u2 = u * u;
	double const u3 = u2 * u;
	double const u4 = u3 * u;
	double const u5 = u4 * u;

	return m_c1 * u + m_c2 * u2 + m_c3 * u3 + m_c4 * u4 + m_c5 * u5;
}

double
XSpline::GBlendFunc::firstDerivative(double u) const
{
	double const u2 = u * u;
	double const u3 = u2 * u;
	double const u4 = u3 * u;

	return m_c1 + 2 * m_c2 * u + 3 * m_c3 * u2 + 4 * m_c4 * u3 + 5 * m_c5 * u4;
}

double
XSpline::GBlendFunc::secondDerivative(double u) const
{
	double const u2 = u * u;
	double const u3 = u2 * u;

	return 2 * m_c2 + 6 * m_c3 * u + 12 * m_c4 * u2 + 20 * m_c5 * u3;
}


/*========================== HBlendFunc ==========================*/

XSpline::HBlendFunc::HBlendFunc(double q)
:	m_c1(q), // See formula 20 in [18].
	m_c2(2 * q),
	m_c4(-2 * q),
	m_c5(-q)
{
}

double
XSpline::HBlendFunc::value(double u) const
{
	double const u2 = u * u;
	double const u3 = u2 * u;
	double const u4 = u3 * u;
	double const u5 = u4 * u;

	return m_c1 * u + m_c2 * u2 + m_c4 * u4 + m_c5 * u5;
}

double
XSpline::HBlendFunc::firstDerivative(double u) const
{
	double const u2 = u * u;
	double const u3 = u2 * u;
	double const u4 = u3 * u;

	return m_c1 + 2 * m_c2 * u + 4 * m_c4 * u3 + 5 * m_c5 * u4;
}

double
XSpline::HBlendFunc::secondDerivative(double u) const
{
	double const u2 = u * u;
	double const u3 = u2 * u;
	
	return 2 * m_c2 + 12 * m_c4 * u2 + 20 * m_c5 * u3;
}


/*======================== PointAndDerivs ========================*/

double
XSpline::PointAndDerivs::signedCurvature() const
{
	double const cross = firstDeriv.x()*secondDeriv.y() - firstDeriv.y()*secondDeriv.x();
	double tlen = sqrt(firstDeriv.x()*firstDeriv.x() + firstDeriv.y()*firstDeriv.y());
	return cross / (tlen * tlen * tlen);
}
