/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (C) 2019, Raspberry Pi (Trading) Limited
 *
 * pwl.cpp - piecewise linear functions
 */

#include <cassert>
#include <stdexcept>

#include "pwl.hpp"

using namespace RPiController;

void Pwl::Read(boost::property_tree::ptree const &params)
{
	for (auto it = params.begin(); it != params.end(); it++) {
		double x = it->second.get_value<double>();
		assert(it == params.begin() || x > points_.back().x);
		it++;
		double y = it->second.get_value<double>();
		points_.push_back(Point(x, y));
	}
	assert(points_.size() >= 2);
}

void Pwl::Append(double x, double y, const double eps)
{
	if (points_.empty() || points_.back().x + eps < x)
		points_.push_back(Point(x, y));
}

void Pwl::Prepend(double x, double y, const double eps)
{
	if (points_.empty() || points_.front().x - eps > x)
		points_.insert(points_.begin(), Point(x, y));
}

Pwl::Interval Pwl::Domain() const
{
	return Interval(points_[0].x, points_[points_.size() - 1].x);
}

Pwl::Interval Pwl::Range() const
{
	double lo = points_[0].y, hi = lo;
	for (auto &p : points_)
		lo = std::min(lo, p.y), hi = std::max(hi, p.y);
	return Interval(lo, hi);
}

bool Pwl::Empty() const
{
	return points_.empty();
}

double Pwl::Eval(double x, int *span_ptr, bool update_span) const
{
	int span = findSpan(x, span_ptr && *span_ptr != -1
				       ? *span_ptr
				       : points_.size() / 2 - 1);
	if (span_ptr && update_span)
		*span_ptr = span;
	return points_[span].y +
	       (x - points_[span].x) * (points_[span + 1].y - points_[span].y) /
		       (points_[span + 1].x - points_[span].x);
}

int Pwl::findSpan(double x, int span) const
{
	// Pwls are generally small, so linear search may well be faster than
	// binary, though could review this if large PWls start turning up.
	int last_span = points_.size() - 2;
	// some algorithms may call us with span pointing directly at the last
	// control point
	span = std::max(0, std::min(last_span, span));
	while (span < last_span && x >= points_[span + 1].x)
		span++;
	while (span && x < points_[span].x)
		span--;
	return span;
}

Pwl::PerpType Pwl::Invert(Point const &xy, Point &perp, int &span,
			  const double eps) const
{
	assert(span >= -1);
	bool prev_off_end = false;
	for (span = span + 1; span < (int)points_.size() - 1; span++) {
		Point span_vec = points_[span + 1] - points_[span];
		double t = ((xy - points_[span]) % span_vec) / span_vec.Len2();
		if (t < -eps) // off the start of this span
		{
			if (span == 0) {
				perp = points_[span];
				return PerpType::Start;
			} else if (prev_off_end) {
				perp = points_[span];
				return PerpType::Vertex;
			}
		} else if (t > 1 + eps) // off the end of this span
		{
			if (span == (int)points_.size() - 2) {
				perp = points_[span + 1];
				return PerpType::End;
			}
			prev_off_end = true;
		} else // a true perpendicular
		{
			perp = points_[span] + span_vec * t;
			return PerpType::Perpendicular;
		}
	}
	return PerpType::None;
}

Pwl Pwl::Inverse(bool *true_inverse, const double eps) const
{
	bool appended = false, prepended = false, neither = false;
	Pwl inverse;