fixed-math-taylor 0.3.0

// Copyright (C) 2026 Jorge Andre Castro
// 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 2 or the License, or
// (at your option) any later version.

#![no_std]

//! # Fixed-Math-Taylor (Modular Edition)
//! 
//! Bibliothèque de trigonométrie haute performance.
//! Activez les moteurs souhaités via les Cargo Features :
//! - `lut` : Virgule fixe Q15 ultra-rapide (recommandé pour MCU).
//! - `taylor` : Série de Taylor (f32) pour la précision.
//! - `fast-sin` : Approximation de Bhaskara I (f32) pour la vitesse.

// --- TYPES DE BASE ---
pub type Angle = u16; // 0..65535 = 0..2π
pub type Fixed = i16; // Q15

// ==========================================
// MOTEUR LUT (FEATURE "lut")
// ==========================================
#[cfg(feature = "lut")]
mod lut_impl {
    use super::{Angle, Fixed};
    const QUADRANT_BITS: u32 = 14;
    const LUT_SIZE: usize = 256;
    const LUT_BITS: u32 = 8;
    const LUT_MASK: u32 = (1 << (QUADRANT_BITS - LUT_BITS)) - 1;

    // Inclusion de la table de sinus (0 à PI/2)
    static SIN_LUT: [Fixed; 257] = include!("sin_table.rs.inc");

    #[inline(always)]
    pub fn sin_fixed(angle: Angle) -> Fixed {
        let quadrant = (angle >> QUADRANT_BITS) as usize;
        let idx = (angle & 0x3FFF) as u32;
        let lut_idx = (idx >> (QUADRANT_BITS - LUT_BITS)) as usize;
        let frac = (idx & LUT_MASK) as i32;

        match quadrant {
            0 => interpolate(SIN_LUT[lut_idx], SIN_LUT[lut_idx + 1], frac),
            1 => interpolate(SIN_LUT[LUT_SIZE - lut_idx], SIN_LUT[LUT_SIZE - lut_idx - 1], frac),
            2 => -interpolate(SIN_LUT[lut_idx], SIN_LUT[lut_idx + 1], frac),
            _ => -interpolate(SIN_LUT[LUT_SIZE - lut_idx], SIN_LUT[LUT_SIZE - lut_idx - 1], frac),
        }
    }

    #[inline(always)]
    fn interpolate(y0: Fixed, y1: Fixed, frac: i32) -> Fixed {
        let y0_32 = y0 as i32;
        let y1_32 = y1 as i32;
        (y0_32 + (((y1_32 - y0_32) * frac) >> (QUADRANT_BITS - LUT_BITS))) as Fixed
    }
}

// Ré-exportation et fonctions publiques liées à la LUT
#[cfg(feature = "lut")]
pub use lut_impl::sin_fixed;

#[cfg(feature = "lut")]
#[inline(always)]
pub fn cos_fixed(angle: Angle) -> Fixed {
    sin_fixed(angle.wrapping_add(16384))
}

#[cfg(feature = "lut")]
#[inline(always)]
pub fn sin_cos(angle: Angle) -> (Fixed, Fixed) {
    (sin_fixed(angle), cos_fixed(angle))
}
// ==========================================
// MOTEUR TAYLOR (Q15 - 100% Entiers)
// ==========================================
#[cfg(feature = "taylor")]
pub mod taylor_impl {
    use super::{Angle, Fixed};
    
    pub fn sin_taylor(angle: Angle) -> Fixed {
        let x_input = if angle > 32768 { 65536 - angle as i32 } else { angle as i32 };
        let x = if x_input > 16384 { 32768 - x_input } else { x_input };

        let x_rad = (x * 51472) >> 14; 

        let x2 = (x_rad * x_rad) >> 15;
        let x3 = (x2 * x_rad) >> 15;
        let x5 = (((x3 * x2) >> 15) * x2) >> 15;

        let term3 = (x3 * 5461) >> 15; 
        let term5 = (x5 * 273) >> 15;

        // C'EST CETTE LIGNE QUI DOIT ÊTRE ICI :
        let res = (x_rad - term3 + term5) as Fixed;
        
        if angle > 32768 { -res } else { res }
    }

    pub fn cos_taylor(angle: super::Angle) -> super::Fixed {
        sin_taylor(angle.wrapping_add(16384))
    }

    

}


// ==========================================
// MOTEUR FAST (Bhaskara I Q15)
// ==========================================
#[cfg(feature = "fast-sin")]
pub mod fast_impl {
    use super::{Angle, Fixed};

    pub fn sin_fast(angle: Angle) -> Fixed {
        // 0..PI (0..32768)
        let x = (angle & 0x7FFF) as i32; 
        let pi = 32768i32;
        
        // num = 4x(pi-x)
        let x_pi_x = (x * (pi - x)) >> 15; // Reste en Q15
        
        // Formule de Bhaskara simplifiée pour calcul entier :
        // sin(x) ≈ (16x(pi-x)) / (5pi^2 - 4x(pi-x))
        let num = (x_pi_x as i64) * 16;
        let den = (5 * 32768) - ((4 * x_pi_x) >> 0); // Approximation du dénominateur
        
        // On scale le numérateur pour la division Q15
        let res = (num * 32767) / den as i64;
        
        let val = res as Fixed;
        if angle > 32768 { -val } else { val }
    }

    pub fn cos_fast(angle: super::Angle) -> super::Fixed {
        sin_fast(angle.wrapping_add(16384))
    }

 
}


// ==========================================
// UTILITAIRES COMMUNS
// ==========================================

#[inline(always)]
pub fn to_fixed(x: f32) -> Fixed { (x * 32767.0) as Fixed }

#[inline(always)]
pub fn from_fixed(x: Fixed) -> f32 { (x as f32) / 32767.0 }

#[inline(always)]
pub fn radians_to_angle(rads: f32) -> Angle {
    let scale = 65536.0 / (2.0 * core::f32::consts::PI);
    (rads * scale) as i32 as u16
}

// ==========================================
// TESTS UNITAIRES
// ==========================================
#[cfg(test)]
mod tests {
    extern crate std;
    use super::*;
    use core::f32::consts::PI;

    #[cfg(feature = "lut")]
    #[test]
    fn test_sin_fixed_precision() {
        // Points cardinaux : Précision exacte (tolérance 1 bit)
        assert!((sin_fixed(0) - 0).abs() <= 1);
        assert!((sin_fixed(16384) - 32767).abs() <= 1); // PI/2 (1.0)
        assert!((sin_fixed(32768) - 0).abs() <= 1);     // PI (0.0)
        assert!((sin_fixed(49152) - (-32767)).abs() <= 1); // 3PI/2 (-1.0)

        // Test à 45°
        let res_raw = sin_fixed(8192); 
        let expected_raw = 23203; 
        assert_eq!(res_raw, expected_raw, "Erreur de précision à 45°");
    }

    #[cfg(feature = "lut")]
    #[test]
    fn test_cos_fixed() {
        // CORRECTION : Appel de cos_fixed au lieu de cos
        assert!((cos_fixed(0) - 32767).abs() <= 1);
        assert!(cos_fixed(16384).abs() <= 1);
        assert!((cos_fixed(32768) - (-32767)).abs() <= 1);
    }

    #[cfg(feature = "taylor")]
    #[test]
    fn test_taylor_accuracy() {
        let res = taylor_impl::sin_taylor(8192); // 45°
        let expected = 23170; 
        assert!((res - expected).abs() < 1000); 
    }

    #[cfg(feature = "fast-sin")]
    #[test]
    fn test_fast_sin_approximation() {
        let res = fast_impl::sin_fast(5461); // 30°
        let expected = 16384; 
        assert!((res - expected).abs() < 1500);
    }

    #[test]
    fn test_radians_to_angle_wrapping() {
        assert_eq!(radians_to_angle(0.0), 0);
        assert_eq!(radians_to_angle(2.0 * PI), 0);
        let a = radians_to_angle(-PI / 2.0);
        assert!(a == 49152 || a == 49151); 
    }

    #[test]
    fn test_fixed_conversion_roundtrip() {
        let original = 0.5f32;
        let fixed = to_fixed(original);
        let back = from_fixed(fixed);
        assert!((original - back).abs() < 0.0001);
    }

    #[test]
    fn test_sin_cos_simultaneous() {
        #[cfg(feature = "lut")]
        {
            let (s, c) = sin_cos(0);
            assert_eq!(s, 0);
            assert_eq!(c, 32767);
        }
    }

    #[test]
    fn test_cos_consistency() {
        let angle_45 = 8192; 
        
        #[cfg(feature = "lut")]
        assert!((cos_fixed(0) - 32767).abs() <= 1);

        #[cfg(feature = "taylor")]
        {
            // CORRECTION : S'assure que cos_taylor est bien appelé
            let res = taylor_impl::cos_taylor(angle_45);
            assert!((res - 23170).abs() < 1000);
        }

        #[cfg(feature = "fast-sin")]
        {
            // CORRECTION : S'assure que cos_fast est bien appelé
            let res = fast_impl::cos_fast(0);
            assert!((res - 32767).abs() < 1500);
        }
    }
}