//! CORDIC fixed-point trigonometry.

//! Implement sine and cosine using the CORDIC algorithm with 16-bit

//! fixed-point arithmetic (Q16.16 format). This is a classic embedded

//! systems / DSP benchmark that stress-tests shift, multiply, and

//! table-driven computation without floating point.

/// Fixed-point scale factor: 1.0 = 65536 (Q16.16).

const SCALE: i32 = 65536;

/// Number of CORDIC iterations (more = more precision).

const ITERATIONS: u32 = 16;

/// CORDIC gain factor in Q16.16. After 16 iterations, the gain is

/// approximately 1/K = 1/1.6468 = 0.60725 * 65536 = 39797.

const CORDIC_GAIN: i32 = 39797;

/// Pi in Q16.16: 3.14159 * 65536 = 205887

const PI: i32 = 205887;

/// Pi/2 in Q16.16.

const HALF_PI: i32 = 102944;

/// Result record for cos and sin.

record CosSin {

    cos: i32,

    sin: i32,

}

/// CORDIC rotation mode: compute cos(angle) and sin(angle).

/// Input angle in Q16.16 radians, must be in [-pi/2, pi/2].

fn cordicRotate(angle: i32, atanTable: *[i32]) -> CosSin {

    let mut x: i32 = CORDIC_GAIN;

    let mut y: i32 = 0;

    let mut z: i32 = angle;

    let mut i: u32 = 0;

    while i < ITERATIONS {

        let dx: i32 = x >> i as i32;

        let dy: i32 = y >> i as i32;

        if z >= 0 {

            x -= dy;

            y += dx;

            z -= atanTable[i];

        } else {

            x += dy;

            y -= dx;

            z += atanTable[i];

        }

        i += 1;

    }

    return CosSin { cos: x, sin: y };

}

/// Compute cos and sin for any angle by reducing to [-pi/2, pi/2].

fn cosSin(angle: i32, atanTable: *[i32]) -> CosSin {

    let mut a: i32 = angle;

    // Reduce to [-pi, pi].

    while a > PI {

        a -= 2 * PI;

    }

    while a < 0 - PI {

        a += 2 * PI;

    }

    // If in [pi/2, pi], use cos(a) = -cos(pi-a), sin(a) = sin(pi-a).

    if a > HALF_PI {

        let r: CosSin = cordicRotate(PI - a, atanTable);

        return CosSin { cos: 0 - r.cos, sin: r.sin };

    } else if a < 0 - HALF_PI {

        // If in [-pi, -pi/2], use cos(a) = -cos(-pi-a), sin(a) = sin(-pi-a)

        let r: CosSin = cordicRotate(0 - PI - a, atanTable);

        return CosSin { cos: 0 - r.cos, sin: r.sin };

    } else {

        return cordicRotate(a, atanTable);

    }

}

/// Absolute value.

fn abs(x: i32) -> i32 {

    if x < 0 {

        return 0 - x;

    }

    return x;

}

/// Fixed-point multiply: (a * b) >> 16, using half-word decomposition

/// to avoid 32-bit overflow.

fn fpmul(a: i32, b: i32) -> i32 {

    // Determine sign and work with magnitudes.

    let neg: bool = (a < 0) != (b < 0);

    let mut ua: u32 = a as u32;

    if a < 0 {

        ua = (0 - a) as u32;

    }

    let mut ub: u32 = b as u32;

    if b < 0 {

        ub = (0 - b) as u32;

    }

    // Split into 16-bit halves: ua = ah:al, ub = bh:bl.

    let al: u32 = ua & 0xFFFF;

    let ah: u32 = ua >> 16;

    let bl: u32 = ub & 0xFFFF;

    let bh: u32 = ub >> 16;

    // Product = ah*bh*2^32 + (ah*bl + al*bh)*2^16 + al*bl

    // We need the result >> 16, so:

    //   (al*bl) >> 16  +  (ah*bl + al*bh)  +  ah*bh*2^16

    let ll: u32 = al * bl;

    let mid: u32 = ah * bl + al * bh;

    let hh: u32 = ah * bh;

    let result: u32 = (ll >> 16) + mid + (hh << 16);

    if neg {

        return 0 - result as i32;

    }

    return result as i32;

}

/// Test cos(0) = 1, sin(0) = 0.

fn testZero(atanTable: *[i32]) -> i32 {

    let r: CosSin = cosSin(0, atanTable);

    // cos(0) should be close to 65536 (1.0 in Q16.16).

    let cosErr: i32 = abs(r.cos - SCALE);

    let sinErr: i32 = abs(r.sin);

    // Allow error of ~1% = 655.

    assert cosErr <= 655;

    assert sinErr <= 655;

    return 0;

}

/// Test cos(pi/2) = 0, sin(pi/2) = 1.

fn testHalfPi(atanTable: *[i32]) -> i32 {

    let r: CosSin = cosSin(HALF_PI, atanTable);

    let cosErr: i32 = abs(r.cos);

    let sinErr: i32 = abs(r.sin - SCALE);

    assert cosErr <= 655;

    assert sinErr <= 655;

    return 0;

}

/// Test cos(pi) = -1, sin(pi) = 0.

fn testPi(atanTable: *[i32]) -> i32 {

    let r: CosSin = cosSin(PI, atanTable);

    let cosErr: i32 = abs(r.cos + SCALE);

    let sinErr: i32 = abs(r.sin);

    assert cosErr <= 655;

    assert sinErr <= 655;

    return 0;

}

/// Test Pythagorean identity: sin^2 + cos^2 = 1 for several angles.

fn testPythagorean(atanTable: *[i32]) -> i32 {

    // Test at 16 evenly spaced angles from 0 to 2*pi.

    let step: i32 = PI / 8;

    let mut i: i32 = 0 - PI;

    while i <= PI {

        let r: CosSin = cosSin(i, atanTable);

        // sin^2 + cos^2 in Q16.16.

        let sin2: i32 = fpmul(r.sin, r.sin);

        let cos2: i32 = fpmul(r.cos, r.cos);

        let sum: i32 = sin2 + cos2;

        let err: i32 = abs(sum - SCALE);

        // Allow ~2% error due to fixed-point precision.

        assert err <= 1311;

        i += step;

    }

    return 0;

}

/// Test symmetry: sin(-x) = -sin(x), cos(-x) = cos(x).

fn testSymmetry(atanTable: *[i32]) -> i32 {

    let angles: [i32; 5] = [16384, 32768, 51472, 65536, 81920];

    let mut i: u32 = 0;

    while i < 5 {

        let a: i32 = angles[i];

        let rPos: CosSin = cosSin(a, atanTable);

        let rNeg: CosSin = cosSin(0 - a, atanTable);

        // cos(-x) should equal cos(x).

        assert abs(rPos.cos - rNeg.cos) <= 655;

        // sin(-x) should equal -sin(x).

        assert abs(rPos.sin + rNeg.sin) <= 655;

        i += 1;

    }

    return 0;

}

/// Test specific known value: cos(pi/3) = 0.5, sin(pi/3) = 0.866.

fn testPiThird(atanTable: *[i32]) -> i32 {

    // pi/3 in Q16.16 = 68629.

    let piThird: i32 = 68629;

    let r: CosSin = cosSin(piThird, atanTable);

    // cos(pi/3) = 0.5 = 32768 in Q16.16.

    let cosErr: i32 = abs(r.cos - 32768);

    // sin(pi/3) = 0.866 = 56756 in Q16.16.

    let sinErr: i32 = abs(r.sin - 56756);

    // Allow ~2% error.

    assert cosErr <= 1311;

    assert sinErr <= 1311;

    return 0;

}

@default fn main() -> i32 {

    /// CORDIC arctangent table in Q16.16 fixed-point.

    let atanTable: [i32; 16] = [

        51472,  // atan(2^0)

        30386,  // atan(2^-1)

        16055,  // atan(2^-2)

        8150,   // atan(2^-3)

        4091,   // atan(2^-4)

        2047,   // atan(2^-5)

        1024,   // atan(2^-6)

        512,    // atan(2^-7)

        256,    // atan(2^-8)

        128,    // atan(2^-9)

        64,     // atan(2^-10)

        32,     // atan(2^-11)

        16,     // atan(2^-12)

        8,      // atan(2^-13)

        4,      // atan(2^-14)

        2       // atan(2^-15)

    ];

    let r1: i32 = testZero(&atanTable[..]);

    if r1 != 0 {

        return 10 + r1;

    }

    let r2: i32 = testHalfPi(&atanTable[..]);

    if r2 != 0 {

        return 20 + r2;

    }

    let r3: i32 = testPi(&atanTable[..]);

    if r3 != 0 {

        return 30 + r3;

    }

    let r4: i32 = testPythagorean(&atanTable[..]);

    if r4 != 0 {

        return 40 + r4;

    }

    let r5: i32 = testSymmetry(&atanTable[..]);

    if r5 != 0 {

        return 50 + r5;

    }

    let r6: i32 = testPiThird(&atanTable[..]);

    if r6 != 0 {

        return 60 + r6;

    }

    return 0;

}