// vast majority of this is taken from Phob firmware

use core::f32::consts::PI;

use defmt::{debug, Format};
use libm::{atan2f, cosf, fabs, fabsf, roundf, sinf, sqrtf};

use crate::{
    config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS},
    helpers::{ToRegularArray, XyValuePair},
    input::Stick,
};

/// fit order for the linearization
const FIT_ORDER: usize = 3;
const NUM_COEFFS: usize = FIT_ORDER + 1;
pub const NO_OF_NOTCHES: usize = 16;
const NO_OF_ADJ_NOTCHES: usize = 12;
pub const NO_OF_CALIBRATION_POINTS: usize = 32;
const MAX_ORDER: usize = 20;

/// 28 degrees; this is the max angular deflection of the stick.
const MAX_STICK_ANGLE: f32 = 0.4886921906;

#[rustfmt::skip]
//                                                             right        notch 1      up right     notch 2      up           notch 3      up left      notch 4      left         notch 5      down left    notch 6      down         notch 7      down right   notch 8
//                                                             0            1            2            3            4            5            6            7            8            9            10           11           12           13           14           15
const CALIBRATION_ORDER: [usize; NO_OF_CALIBRATION_POINTS] = [ 0, 1,        8, 9,       16, 17,       24, 25,      4, 5,        12, 13,      20, 21,      28, 29,      2, 3,        6, 7,        10, 11,      14, 15,      18, 19,      22, 23,      26, 27,      30, 31 ];

#[rustfmt::skip]
//                                                          up right     up left      down left    down right   notch 1      notch 2      notch 3      notch 4      notch 5      notch 6      notch 7      notch 8
const NOTCH_ADJUSTMENT_ORDER: [usize; NO_OF_ADJ_NOTCHES] = [2,           6,           10,          14,          1,           3,           5,           7,           9,           11,          13,          15];

#[derive(Clone, Debug, Default, Format)]
pub struct StickParams {
    // these are the linearization coefficients
    pub fit_coeffs: XyValuePair<[f32; NUM_COEFFS]>,

    // these are the notch remap parameters
    pub affine_coeffs: [[f32; 4]; 16], // affine transformation coefficients for all regions of the stick
    pub boundary_angles: [f32; 16], // angles at the boundaries between regions of the stick (in the plane)
}

impl StickParams {
    /// Generate StickParams structs for the sticks, returned as a tuple of (analog_stick, c_stick)
    pub fn from_stick_config(stick_config: &StickConfig) -> Self {
        let cleaned_cal_points = CleanedCalibrationPoints::from_temp_calibration_points(
            &stick_config.temp_cal_points_x.to_regular_array(),
            &stick_config.temp_cal_points_y.to_regular_array(),
            &stick_config.angles.to_regular_array(),
        );

        let linearized_cal = LinearizedCalibration::from_calibration_points(&cleaned_cal_points);

        let notch_cal = NotchCalibration::from_cleaned_and_linearized_calibration(
            &cleaned_cal_points,
            &linearized_cal,
        );

        Self {
            fit_coeffs: XyValuePair {
                x: linearized_cal.fit_coeffs.x.map(|e| e as f32),
                y: linearized_cal.fit_coeffs.y.map(|e| e as f32),
            },
            affine_coeffs: notch_cal.affine_coeffs,
            boundary_angles: notch_cal.boundary_angles,
        }
    }
}

#[derive(Clone, Debug, Format, Copy)]
pub enum NotchStatus {
    TertInactive,
    TertActive,
    Secondary,
    Cardinal,
}

#[derive(Clone, Debug)]
struct CleanedCalibrationPoints {
    pub cleaned_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
    pub notch_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
    pub notch_status: [NotchStatus; NO_OF_NOTCHES],
}

impl Default for CleanedCalibrationPoints {
    fn default() -> Self {
        Self {
            cleaned_points: XyValuePair {
                x: [0f32; NO_OF_NOTCHES + 1],
                y: [0f32; NO_OF_NOTCHES + 1],
            },
            notch_points: XyValuePair {
                x: [0f32; NO_OF_NOTCHES + 1],
                y: [0f32; NO_OF_NOTCHES + 1],
            },
            notch_status: DEFAULT_NOTCH_STATUS,
        }
    }
}

impl CleanedCalibrationPoints {
    pub fn from_temp_calibration_points(
        cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS],
        cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS],
        notch_angles: &[f32; NO_OF_NOTCHES],
    ) -> Self {
        let mut out = Self::default();

        debug!("Raw calibration points:");
        for i in 0..NO_OF_CALIBRATION_POINTS {
            debug!("({}, {})", cal_points_x[i], cal_points_y[i])
        }

        debug!("Notch angles: {}", notch_angles);

        for i in 0..NO_OF_NOTCHES {
            // add the origin values to the first x,y point
            out.cleaned_points.x[0] += cal_points_x[i * 2];
            out.cleaned_points.y[0] += cal_points_y[i * 2];

            // copy the cal point into the cleaned list
            out.cleaned_points.x[i + 1] = cal_points_x[i * 2 + 1];
            out.cleaned_points.y[i + 1] = cal_points_y[i * 2 + 1];

            (out.notch_points.x[i + 1], out.notch_points.y[i + 1]) =
                match calc_stick_values(notch_angles[i]) {
                    (a, b) => (roundf(a), roundf(b)),
                };
        }

        // TODO: put the below in a macro to clean it up a bit, once it's confirmed to work
        // remove the largest and smallest two origin values to remove outliers
        // first, find their indices
        let mut i = 0;
        let x_by_size = &mut cal_points_x.map(|e| {
            i += 1;
            (i - 1, e)
        });

        tiny_sort::unstable::sort_by(x_by_size, |a, b| a.1.partial_cmp(&b.1).unwrap());

        let smallest_x = x_by_size[0].0;
        let small_x = x_by_size[1].0;
        let large_x = x_by_size[x_by_size.len() - 2].0;
        let largest_x = x_by_size[x_by_size.len() - 1].0;

        // do the same for y
        let mut i = 0;
        let y_by_size = &mut cal_points_y.map(|e| {
            i += 1;
            (i - 1, e)
        });

        tiny_sort::unstable::sort_by(y_by_size, |a, b| a.1.partial_cmp(&b.1).unwrap());

        let smallest_y = y_by_size[0].0;
        let small_y = y_by_size[1].0;
        let large_y = y_by_size[y_by_size.len() - 2].0;
        let largest_y = y_by_size[y_by_size.len() - 1].0;

        // TODO: make this whole thing a function? it looks very ugly
        out.cleaned_points.x[0] -= cal_points_x[smallest_x];
        out.cleaned_points.x[0] -= cal_points_x[small_x];
        out.cleaned_points.x[0] -= cal_points_x[large_x];
        out.cleaned_points.x[0] -= cal_points_x[largest_x];

        out.cleaned_points.y[0] -= cal_points_y[smallest_y];
        out.cleaned_points.y[0] -= cal_points_y[small_y];
        out.cleaned_points.y[0] -= cal_points_y[large_y];
        out.cleaned_points.y[0] -= cal_points_y[largest_y];

        out.cleaned_points.x[0] /= (NO_OF_NOTCHES - 4) as f32;
        out.cleaned_points.y[0] /= (NO_OF_NOTCHES - 4) as f32;

        for i in 0..NO_OF_NOTCHES {
            let delta_x = out.cleaned_points.x[i + 1] - out.cleaned_points.x[0];
            let delta_y = out.cleaned_points.y[i + 1] - out.cleaned_points.y[0];
            let mag = sqrtf(delta_x * delta_x + delta_y * delta_y);

            // if the cleaned point was at the center and would be a firefox notch
            // average the previous and next points (cardinal & diagonal) for some sanity
            if mag < 0.02 && (i % 2 == 0) {
                let prev_index = ((i + NO_OF_NOTCHES - 1) % NO_OF_NOTCHES) + 1;
                let next_index = ((i + 1) % NO_OF_NOTCHES) + 1;

                out.cleaned_points.x[i + 1] =
                    (out.cleaned_points.x[prev_index] + out.cleaned_points.x[next_index]) / 2.0;
                out.cleaned_points.y[i + 1] =
                    (out.cleaned_points.y[prev_index] + out.cleaned_points.y[next_index]) / 2.0;

                out.notch_points.x[i + 1] =
                    (out.notch_points.x[prev_index] + out.notch_points.x[next_index]) / 2.0;
                out.notch_points.y[i + 1] =
                    (out.notch_points.y[prev_index] + out.notch_points.y[next_index]) / 2.0;

                debug!("Skipping notch {}", i + 1);

                // Mark that notch adjustment should be skipped for this
                out.notch_status[i] = NotchStatus::TertInactive;
            } else {
                out.notch_status[i] = DEFAULT_NOTCH_STATUS[i];
            }
        }

        debug!("Final points:");
        for i in 0..=NO_OF_NOTCHES {
            debug!(
                "Cleaned: ({}, {}), Notch: ({}, {})",
                out.cleaned_points.x[i],
                out.cleaned_points.y[i],
                out.notch_points.x[i],
                out.notch_points.y[i],
            );
        }

        debug!("The notch statuses are: {:?}", out.notch_status);

        out
    }
}

#[derive(Clone, Debug, Default)]
struct LinearizedCalibration {
    pub fit_coeffs: XyValuePair<[f64; NUM_COEFFS]>,

    pub linearized_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
}

impl LinearizedCalibration {
    ///
    /// Generate a fit to linearize the stick response.
    ///
    /// Inputs:
    ///     cleaned points X and Y, (must be 17 points for each of these, the first being the center, the others starting at 3 oclock and going around counterclockwise)
    ///
    ///	Outputs:
    ///		linearization fit coefficients for X and Y
    pub fn from_calibration_points(cleaned_calibration_points: &CleanedCalibrationPoints) -> Self {
        let mut fit_points_x = [0f64; 5];
        let mut fit_points_y = [0f64; 5];

        let in_x = cleaned_calibration_points
            .cleaned_points
            .x
            .map(|e| e as f64);
        let in_y = cleaned_calibration_points
            .cleaned_points
            .y
            .map(|e| e as f64);

        fit_points_x[0] = in_x[8 + 1];
        fit_points_x[1] = (in_x[6 + 1] + in_x[10 + 1]) / 2.0f64;
        fit_points_x[2] = in_x[0];
        fit_points_x[3] = (in_x[2 + 1] + in_x[14 + 1]) / 2.0f64;
        fit_points_x[4] = in_x[0 + 1];

        fit_points_y[0] = in_y[12 + 1];
        fit_points_y[1] = (in_y[10 + 1] + in_y[14 + 1]) / 2.0f64;
        fit_points_y[2] = in_y[0];
        fit_points_y[3] = (in_y[6 + 1] + in_y[2 + 1]) / 2.0f64;
        fit_points_y[4] = in_y[4 + 1];

        let x_output: [f64; 5] = [27.5, 53.2537879754, 127.5, 201.7462120246, 227.5];
        let y_output: [f64; 5] = [27.5, 53.2537879754, 127.5, 201.7462120246, 227.5];

        let mut fit_coeffs_x =
            fit_curve::<5, NUM_COEFFS>(FIT_ORDER as i32, &fit_points_x, &x_output);
        let mut fit_coeffs_y =
            fit_curve::<5, NUM_COEFFS>(FIT_ORDER as i32, &fit_points_y, &y_output);

        let x_zero_error = linearize(fit_points_x[2] as f32, &fit_coeffs_x.map(|e| e as f32));
        let y_zero_error = linearize(fit_points_y[2] as f32, &fit_coeffs_y.map(|e| e as f32));

        fit_coeffs_x[3] = fit_coeffs_x[3] - x_zero_error as f64;
        fit_coeffs_y[3] = fit_coeffs_y[3] - y_zero_error as f64;

        let mut linearized_points_x = [0f32; NO_OF_NOTCHES + 1];
        let mut linearized_points_y = [0f32; NO_OF_NOTCHES + 1];

        for i in 0..=NO_OF_NOTCHES {
            linearized_points_x[i] = linearize(in_x[i] as f32, &fit_coeffs_x.map(|e| e as f32));
            linearized_points_y[i] = linearize(in_y[i] as f32, &fit_coeffs_y.map(|e| e as f32));
        }

        Self {
            fit_coeffs: XyValuePair {
                x: fit_coeffs_x,
                y: fit_coeffs_y,
            },
            linearized_points: XyValuePair {
                x: linearized_points_x,
                y: linearized_points_y,
            },
        }
    }
}

#[derive(Clone, Debug, Default)]
struct NotchCalibration {
    affine_coeffs: [[f32; 4]; 16],
    boundary_angles: [f32; 16],
}

impl NotchCalibration {
    fn from_cleaned_and_linearized_calibration(
        cleaned_calibration_points: &CleanedCalibrationPoints,
        linearized_calibration: &LinearizedCalibration,
    ) -> Self {
        let mut out = Self::default();

        for i in 1..=NO_OF_NOTCHES {
            let mut points_in = [[0f32; 3]; 3];
            let mut points_out = [[0f32; 3]; 3];

            if i == NO_OF_NOTCHES {
                points_in[0][0] = linearized_calibration.linearized_points.x[0];
                points_in[0][1] = linearized_calibration.linearized_points.x[i];
                points_in[0][2] = linearized_calibration.linearized_points.x[1];
                points_in[1][0] = linearized_calibration.linearized_points.y[0];
                points_in[1][1] = linearized_calibration.linearized_points.y[i];
                points_in[1][2] = linearized_calibration.linearized_points.y[1];
                points_in[2][0] = 1.;
                points_in[2][1] = 1.;
                points_in[2][2] = 1.;
                points_out[0][0] = cleaned_calibration_points.notch_points.x[0];
                points_out[0][1] = cleaned_calibration_points.notch_points.x[i];
                points_out[0][2] = cleaned_calibration_points.notch_points.x[1];
                points_out[1][0] = cleaned_calibration_points.notch_points.y[0];
                points_out[1][1] = cleaned_calibration_points.notch_points.y[i];
                points_out[1][2] = cleaned_calibration_points.notch_points.y[1];
                points_out[2][0] = 1.;
                points_out[2][1] = 1.;
                points_out[2][2] = 1.;
            } else {
                points_in[0][0] = linearized_calibration.linearized_points.x[0];
                points_in[0][1] = linearized_calibration.linearized_points.x[i];
                points_in[0][2] = linearized_calibration.linearized_points.x[i + 1];
                points_in[1][0] = linearized_calibration.linearized_points.y[0];
                points_in[1][1] = linearized_calibration.linearized_points.y[i];
                points_in[1][2] = linearized_calibration.linearized_points.y[i + 1];
                points_in[2][0] = 1.;
                points_in[2][1] = 1.;
                points_in[2][2] = 1.;
                points_out[0][0] = cleaned_calibration_points.notch_points.x[0];
                points_out[0][1] = cleaned_calibration_points.notch_points.x[i];
                points_out[0][2] = cleaned_calibration_points.notch_points.x[i + 1];
                points_out[1][0] = cleaned_calibration_points.notch_points.y[0];
                points_out[1][1] = cleaned_calibration_points.notch_points.y[i];
                points_out[1][2] = cleaned_calibration_points.notch_points.y[i + 1];
                points_out[2][0] = 1.;
                points_out[2][1] = 1.;
                points_out[2][2] = 1.;
            }
            debug!("In points: {:?}", points_in);
            debug!("Out points: {:?}", points_out);

            let temp = inverse(&points_in);

            let a = matrix_mult(&points_out, &temp);

            debug!("The transform matrix is: {:?}", a);

            for j in 0..2 {
                for k in 0..2 {
                    out.affine_coeffs[i - 1][j * 2 + k] = a[j][k];
                }
            }

            debug!(
                "Transform coefficients for this region are: {:?}",
                out.affine_coeffs[i - 1]
            );

            out.boundary_angles[i - 1] = match atan2f(
                linearized_calibration.linearized_points.y[i]
                    - linearized_calibration.linearized_points.y[0],
                linearized_calibration.linearized_points.x[i]
                    - linearized_calibration.linearized_points.x[0],
            ) {
                a if a < out.boundary_angles[0] => a + 2. * PI,
                a => a,
            };
        }

        out
    }
}

fn inverse(in_mat: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
    let mut out_mat = [[0f32; 3]; 3];

    let det = in_mat[0][0] * (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1])
        - in_mat[0][1] * (in_mat[1][0] * in_mat[2][2] - in_mat[1][2] * in_mat[2][0])
        + in_mat[0][2] * (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]);

    out_mat[0][0] = (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1]) / det;
    out_mat[0][1] = (in_mat[0][2] * in_mat[2][1] - in_mat[0][1] * in_mat[2][2]) / det;
    out_mat[0][2] = (in_mat[0][1] * in_mat[1][2] - in_mat[0][2] * in_mat[1][1]) / det;
    out_mat[1][0] = (in_mat[1][2] * in_mat[2][0] - in_mat[1][0] * in_mat[2][2]) / det;
    out_mat[1][1] = (in_mat[0][0] * in_mat[2][2] - in_mat[0][2] * in_mat[2][0]) / det;
    out_mat[1][2] = (in_mat[0][2] * in_mat[1][0] - in_mat[0][0] * in_mat[1][2]) / det;
    out_mat[2][0] = (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]) / det;
    out_mat[2][1] = (in_mat[0][1] * in_mat[2][0] - in_mat[0][0] * in_mat[2][1]) / det;
    out_mat[2][2] = (in_mat[0][0] * in_mat[1][1] - in_mat[0][1] * in_mat[1][0]) / det;

    out_mat
}

fn matrix_mult(a: &[[f32; 3]; 3], b: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
    let mut out = [[0f32; 3]; 3];

    for i in 0..3 {
        for j in 0..3 {
            for k in 0..3 {
                out[i][j] += a[i][k] * b[k][j];
            }
        }
    }

    out
}

/// Calculate the power of a number
fn curve_fit_power(base: f64, exponent: u32) -> f64 {
    if exponent == 0 {
        return 1.0;
    }

    let mut val = base;

    for _ in 1..exponent {
        val *= base;
    }

    val
}

/// Substitutes a column in a matrix with a vector
fn sub_col<const N: usize>(
    matrix: &[[f64; N]; N],
    t: &[f64; MAX_ORDER],
    col: usize,
    n: usize,
) -> [[f64; N]; N] {
    let mut m = *matrix;

    for i in 0..n {
        m[i][col] = t[i];
    }

    m
}

/// Calculate the determinant of a matrix
fn det<const N: usize>(matrix: &[[f64; N]; N]) -> f64 {
    let sign = trianglize(matrix);

    if sign == 0 {
        return 0.;
    }

    let mut p = 1f64;

    for i in 0..N {
        p *= matrix[i][i];
    }

    p * (sign as f64)
}

/// Trianglize a matrix
fn trianglize<const N: usize>(matrix: &[[f64; N]; N]) -> i32 {
    let mut sign = 1;
    let mut matrix = *matrix;

    for i in 0..N {
        let mut max = 0;
        for row in i..N {
            if fabs(matrix[row][i]) > fabs(matrix[max][i]) {
                max = row;
            }
        }
        if max > 0 {
            sign = -sign;
            let tmp = matrix[i];
            matrix[i] = matrix[max];
            matrix[max] = tmp;
        }
        if matrix[i][i] == 0. {
            return 0;
        }
        for row in i + 1..N {
            let factor = matrix[row][i] / matrix[i][i];
            if factor == 0. {
                continue;
            }
            for col in i..N {
                matrix[row][col] -= factor * matrix[i][col];
            }
        }
    }

    sign
}

fn fit_curve<const N: usize, const NCOEFFS: usize>(
    order: i32,
    px: &[f64; N],
    py: &[f64; N],
) -> [f64; NCOEFFS] {
    let mut coeffs = [0f64; NCOEFFS];

    if NCOEFFS != (order + 1) as usize {
        panic!(
            "Invalid coefficients length, expected {}, but got {}",
            order + 1,
            NCOEFFS
        );
    }

    if NCOEFFS > MAX_ORDER || NCOEFFS < 2 {
        panic!("Matrix size out of bounds");
    }

    if N < 1 {
        panic!("Not enough points to fit");
    }

    let mut t = [0f64; MAX_ORDER];
    let mut s = [0f64; MAX_ORDER * 2 + 1];

    for i in 0..N {
        let x = px[i];
        let y = py[i];
        for j in 0..NCOEFFS * 2 - 1 {
            s[j] += curve_fit_power(x, j as u32);
        }
        for j in 0..NCOEFFS {
            t[j] += y * curve_fit_power(x, j as u32);
        }
    }

    //Master matrix LHS of linear equation
    let mut matrix = [[0f64; NCOEFFS]; NCOEFFS];

    for i in 0..NCOEFFS {
        for j in 0..NCOEFFS {
            matrix[i][j] = s[i + j];
        }
    }

    let denom = det(&matrix);

    for i in 0..NCOEFFS {
        coeffs[NCOEFFS - i - 1] = det(&sub_col(&matrix, &t, i, NCOEFFS)) / denom;
    }

    coeffs
}

/// Compute the stick x/y coordinates from a given angle.
/// The stick moves spherically, so it requires 3D trigonometry.
fn calc_stick_values(angle: f32) -> (f32, f32) {
    let x =
        100. * atan2f(sinf(MAX_STICK_ANGLE) * cosf(angle), cosf(MAX_STICK_ANGLE)) / MAX_STICK_ANGLE;
    let y =
        100. * atan2f(sinf(MAX_STICK_ANGLE) * sinf(angle), cosf(MAX_STICK_ANGLE)) / MAX_STICK_ANGLE;

    (x, y)
}

pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 {
    coefficients[0] * (point * point * point)
        + coefficients[1] * (point * point)
        + coefficients[2] * point
        + coefficients[3]
}

pub fn notch_remap(
    x_in: f32,
    y_in: f32,
    stick_params: &StickParams,
    controller_config: &ControllerConfig,
    which_stick: Stick,
    is_calibrating: bool,
) -> (f32, f32) {
    //determine the angle between the x unit vector and the current position vector
    let angle = match atan2f(y_in, x_in) {
        //unwrap the angle based on the first region boundary
        a if a < stick_params.boundary_angles[0] => a + PI * 2.0,
        a => a,
    };

    //go through the region boundaries from lowest angle to highest, checking if the current position vector is in that region
    //if the region is not found then it must be between the first and the last boundary, ie the last region
    //we check GATE_REGIONS*2 because each notch has its own very small region we use to make notch values more consistent
    let region = 'a: {
        for i in 1..NO_OF_NOTCHES {
            if angle < stick_params.boundary_angles[i] {
                break 'a i - 1;
            }
        }
        NO_OF_NOTCHES - 1
    };

    let stick_scale = match which_stick {
        Stick::ControlStick => controller_config.astick_config.analog_scaler as f32 / 100.,
        Stick::CStick => controller_config.cstick_config.analog_scaler as f32 / 100.,
    };

    let mut x_out = stick_scale
        * (stick_params.affine_coeffs[region][0] * x_in
            + stick_params.affine_coeffs[region][1] * y_in);
    let mut y_out = stick_scale
        * (stick_params.affine_coeffs[region][2] * x_in
            + stick_params.affine_coeffs[region][3] * y_in);

    if !is_calibrating {
        let stick_config = match which_stick {
            Stick::ControlStick => &controller_config.astick_config,
            Stick::CStick => &controller_config.cstick_config,
        };

        if stick_config.cardinal_snapping > 0 {
            if fabsf(x_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(y_out) >= 79.5 {
                x_out = 0.;
            }
            if fabsf(y_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(x_out) >= 79.5 {
                y_out = 0.;
            }
        } else if stick_config.cardinal_snapping == -1 {
            if fabsf(x_out) < 6.5 && fabsf(y_out) >= 79.5 {
                x_out = 0.;
            }
            if fabsf(y_out) < 6.5 && fabsf(x_out) >= 79.5 {
                y_out = 0.;
            }
        }

        if fabsf(x_out) < 3. && fabsf(y_out) < 3. {
            x_out = 0.;
            y_out = 0.;
        }
    }

    (x_out, y_out)
}