// -*- coding: utf-8 -*-
//
// Copyright 2021 Michael Büsch <m@bues.ch>
//
// Licensed under the Apache License version 2.0
// or the MIT license, at your option.
// SPDX-License-Identifier: Apache-2.0 OR MIT
//

use crate::base::PhyType;

#[derive(Clone, Copy, PartialEq, Debug)]
pub struct CurvePoint {
    x: PhyType,
    y: PhyType,
}

impl CurvePoint {
    pub const fn new(x: PhyType, y: PhyType) -> CurvePoint {
        CurvePoint { x, y }
    }
}

#[derive(Clone, Debug)]
pub struct CurveIpo {
    points: Vec<CurvePoint>,
}

impl CurveIpo {
    pub const fn new(points: Vec<CurvePoint>) -> CurveIpo {
        CurveIpo { points }
    }

    pub fn set_points(&mut self, points: Vec<CurvePoint>) {
        self.points = points;
    }

    pub fn interpolate(&self, x: PhyType) -> PhyType {
        assert!(!self.points.is_empty());
        let first = self.points[0];
        let last = self.points[self.points.len() - 1];
        if x <= first.x {
            first.y
        } else if x >= last.x {
            last.y
        } else if !x.is_finite() {
            x
        } else {
            // Find the curve points
            // left handed and right handed to the x value.
            let mut lhp_found = first;
            let mut rhp_found = first;
            for rhp in &self.points {
                if rhp.x >= x {
                    rhp_found = *rhp;
                    break;
                }
                lhp_found = *rhp;
            }
            let lx = lhp_found.x;
            let ly = lhp_found.y;
            let rx = rhp_found.x;
            let ry = rhp_found.y;

            debug_assert!(rx - lx > 0.into());
            // Linear interpolation between lhp and rhp:
            ((x - lx) * ((ry - ly) / (rx - lx))) + ly
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_base() {
        let a = CurvePoint::new(1.into(), 2.into());
        assert_eq!(a.x, 1.into());
        assert_eq!(a.y, 2.into());

        let mut a = CurveIpo::new(vec![CurvePoint::new(1.into(), 2.into())]);
        assert_eq!(a.points.len(), 1);
        assert_eq!(a.points[0].x, 1.into());
        assert_eq!(a.points[0].y, 2.into());

        a.set_points(vec![
            CurvePoint::new(3.into(), 4.into()),
            CurvePoint::new(5.into(), 6.into()),
        ]);
        assert_eq!(a.points.len(), 2);
        assert_eq!(a.points[0].x, 3.into());
        assert_eq!(a.points[0].y, 4.into());
        assert_eq!(a.points[1].x, 5.into());
        assert_eq!(a.points[1].y, 6.into());
    }

    #[test]
    fn test_interpolate() {
        let a = CurveIpo::new(vec![
            CurvePoint::new(1.into(), 1.into()),
            CurvePoint::new(2.into(), 2.into()),
            CurvePoint::new(10.into(), 20.into()),
            CurvePoint::new(20.into(), (-10).into()),
        ]);
        // out of bounds:
        assert_eq!(a.interpolate((-1000).into()), 1.into());
        assert_eq!(a.interpolate(0.into()), 1.into());
        // in bounds:
        assert_eq!(a.interpolate(1.into()), 1.into());
        assert_eq!(a.interpolate(1.5.into()), 1.5.into());
        assert_eq!(a.interpolate(2.into()), 2.into());
        assert_eq!(a.interpolate(3.into()), 4.25.into());
        assert_eq!(a.interpolate(10.into()), 20.into());
        assert_eq!(a.interpolate(12.into()), 14.into());
        assert_eq!(a.interpolate(20.into()), (-10).into());
        // out of bounds:
        assert_eq!(a.interpolate(21.into()), (-10).into());
        assert_eq!(a.interpolate(1000.into()), (-10).into());

        // Single point
        let a = CurveIpo::new(vec![CurvePoint::new(2.into(), 20.into())]);
        assert_eq!(a.interpolate(1.into()), 20.into());
        assert_eq!(a.interpolate(2.into()), 20.into());
        assert_eq!(a.interpolate(3.into()), 20.into());
    }
}

// vim: ts=4 sw=4 expandtab