#!/usr/bin/env python3
"""
Generate golden test vectors for CORDIC sin/cos benchmark.

Input format: 16-bit signed Q2.14 (angle in radians)
Output format: 16-bit signed Q1.15 (sin/cos values)

Range: [-π/2, +π/2]
"""

import math

def float_to_q214(val):
    """Convert float to 16-bit Q2.14 signed integer."""
    scaled = int(round(val * (2**14)))
    # Clamp to 16-bit signed range
    if scaled > 32767:
        scaled = 32767
    if scaled < -32768:
        scaled = -32768
    return scaled & 0xFFFF

def q214_to_float(val):
    """Convert 16-bit Q2.14 hex value back to float."""
    if val >= 0x8000:
        val = val - 0x10000
    return val / (2**14)

def float_to_q115(val):
    """Convert float [-1, 1) to 16-bit Q1.15 signed integer."""
    scaled = int(round(val * (2**15)))
    # Clamp to valid Q1.15 range
    if scaled >= 32768:
        scaled = 32767
    if scaled < -32768:
        scaled = -32768
    return scaled & 0xFFFF

def generate_test_vectors():
    """Generate comprehensive test vectors for CORDIC verification."""
    test_angles_float = []
    
    # Range is stricter now: [-π/2, +π/2] approx [-1.570796, +1.570796]
    MAX_ANGLE = 1.5707
    
    # Boundary angles
    boundary_angles = [
        0.0,
        1.5707,                 # pi/2
        -1.5707,                # -pi/2
        math.pi / 6,            # 0.523...
        math.pi / 4,            # 0.785...
        math.pi / 3,            # 1.047...
        -math.pi / 6,
        -math.pi / 4,
        -math.pi / 3,
    ]
    
    test_angles_float.extend(boundary_angles)
    
    # Sweep
    for i in range(50):
        angle = -MAX_ANGLE + (2 * MAX_ANGLE) * i / 49
        test_angles_float.append(angle)
    
    # Convert to Q2.14 and deduplicate
    seen_q214 = set()
    vectors = []
    
    for angle in test_angles_float:
        angle_q214 = float_to_q214(angle)
        
        # Ensure within strict range if needed (though clamping handles overflow, we want to stay within valid CORDIC range)
        
        if angle_q214 in seen_q214:
            continue
        seen_q214.add(angle_q214)
        
        actual_angle = q214_to_float(angle_q214)
        sin_val = math.sin(actual_angle)
        cos_val = math.cos(actual_angle)
        
        sin_q115 = float_to_q115(sin_val)
        cos_q115 = float_to_q115(cos_val)
        vectors.append((angle_q214, sin_q115, cos_q115))
    
    # Sort
    vectors.sort(key=lambda x: x[0] if x[0] < 0x8000 else x[0] - 0x10000)
    
    return vectors

def generate_verilog_arrays():
    """Generate Verilog array initialization code."""
    vectors = generate_test_vectors()
    
    lines = []
    lines.append(f"// Auto-generated golden vectors: {len(vectors)} test cases")
    lines.append(f"// Format: angle (Q2.14), expected_sin (Q1.15), expected_cos (Q1.15)")
    lines.append(f"localparam NUM_TESTS = {len(vectors)};")
    lines.append("")
    
    # Angle array
    lines.append("// Input angles (Q2.14 radians)")
    lines.append("reg [15:0] test_angles [0:NUM_TESTS-1];")
    lines.append("initial begin")
    for i, (angle, sin_val, cos_val) in enumerate(vectors):
        actual_angle = q214_to_float(angle)
        lines.append(f"    test_angles[{i}] = 16'h{angle:04X}; // {actual_angle:.6f} rad")
    lines.append("end")
    lines.append("")
    
    # Expected sin array
    lines.append("// Expected sin values (Q1.15)")
    lines.append("reg [15:0] expected_sin [0:NUM_TESTS-1];")
    lines.append("initial begin")
    for i, (angle, sin_val, cos_val) in enumerate(vectors):
        lines.append(f"    expected_sin[{i}] = 16'h{sin_val:04X};")
    lines.append("end")
    lines.append("")
    
    # Expected cos array
    lines.append("// Expected cos values (Q1.15)")
    lines.append("reg [15:0] expected_cos [0:NUM_TESTS-1];")
    lines.append("initial begin")
    for i, (angle, sin_val, cos_val) in enumerate(vectors):
        lines.append(f"    expected_cos[{i}] = 16'h{cos_val:04X};")
    lines.append("end")
    
    return "\n".join(lines)

if __name__ == "__main__":
    print(generate_verilog_arrays())