//! returns: 0
//! 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;
}