#include <Plaquette.h>

#include <stdint.h>

#include <stdbool.h>

#define RATIONAL_MAX UINT32_MAX

typedef uint32_t rational01_t;

// Initialization
rational01_t rational_from_float(float f);
float rational_to_float(rational01_t r);

// Arithmetic operations
rational01_t rational_add(rational01_t a, rational01_t b);
rational01_t rational_sub(rational01_t a, rational01_t b);
rational01_t rational_mul(rational01_t a, rational01_t b);
rational01_t rational_div(rational01_t a, rational01_t b);

// Comparison operations
bool rational_equal(rational01_t a, rational01_t b);
bool rational_less(rational01_t a, rational01_t b);
bool rational_greater(rational01_t a, rational01_t b);

#include <string.h>

// Convert a float in [0, 1] to rational01_t
rational01_t rational_from_float(float f) {
    if (f < 0.0f) f = 0.0f;
    if (f > 1.0f) f = 1.0f;
    return (rational01_t)(f * RATIONAL_MAX);
}

// Convert rational01_t to float using platform-specific optimizations
float rational_to_float(rational01_t r) {
#if defined(__STDC_IEC_559__) || defined(__IEEE754__) || defined(ESP_PLATFORM) || defined(TEENSYDUINO) || defined(__AVR__)
    union {
        uint32_t i;
        float f;
    } u;

    u.i = (127 << 23) | (r >> 9); // Set exponent for 1.0 and shift mantissa
    u.i &= 0x7FFFFF;              // Clear the implicit leading 1
    return u.f;
#else
    // Portable fallback for unknown platforms
    return (float)r / (float)RATIONAL_MAX;
#endif
}

// Addition
rational01_t rational_add(rational01_t a, rational01_t b) {
    uint64_t sum = (uint64_t)a + (uint64_t)b;
    if (sum > RATIONAL_MAX) sum = RATIONAL_MAX; // Clamp to max
    return (rational01_t)sum;
}

// Subtraction
rational01_t rational_sub(rational01_t a, rational01_t b) {
    int64_t diff = (int64_t)a - (int64_t)b;
    if (diff < 0) diff = 0; // Clamp to zero
    return (rational01_t)diff;
}

// Multiplication
rational01_t rational_mul(rational01_t a, rational01_t b) {
    uint64_t product = (uint64_t)a * (uint64_t)b;
    return (rational01_t)(product / RATIONAL_MAX);
}

// Division
rational01_t rational_div(rational01_t a, rational01_t b) {
    if (b == 0) {
        return RATIONAL_MAX; // Represent infinity
    }
    uint64_t dividend = ((uint64_t)a * RATIONAL_MAX);
    return (rational01_t)(dividend / b);
}

// Comparison
bool rational_equal(rational01_t a, rational01_t b) {
    return a == b;
}

bool rational_less(rational01_t a, rational01_t b) {
    return a < b;
}

bool rational_greater(rational01_t a, rational01_t b) {
    return a > b;
}

// Function to compute fast sine approximation using rational01_t
rational01_t rational_sin32(rational01_t theta);

// Constants for sine approximation (scaled for fixed-point math)
#define PI_2 (RATIONAL_MAX / 2)
#define PI   (RATIONAL_MAX)
#define PI_3 (3 * PI_2)

// Fast sine approximation using cubic polynomial: y = 4x(1 - x)
// Adjusted for better accuracy: y = 4x(1 - x)(1.273 - 0.405 * x)
// where x = theta in [0, 1] mapped to [0, PI/2]

rational01_t rational_sin32(rational01_t theta) {
    // Normalize to 0 - 2PI range
    uint32_t angle = theta;
    uint8_t quadrant = (angle >> 30) & 0x3; // Determine the quadrant

    // Normalize angle to first quadrant (0 to PI/2)
    angle &= 0x3FFFFFFF; // Mask to 30 bits
    if (quadrant == 1 || quadrant == 3) {
        angle = (1UL << 30) - angle; // Reflect for 2nd and 4th quadrants
    }

    // Fixed-point approximation of sine using a parabolic approximation
    // y ≈ 4x(1 - x)
    uint64_t x = angle;
    uint64_t y = (4ULL * x * ((1UL << 30) - x)) >> 30;  // Scale back to 32 bits

    // Apply sign based on quadrant
    if (quadrant >= 2) {
        y = (RATIONAL_MAX - y) & RATIONAL_MAX; // Apply negative sign
    }

    return (rational01_t)y;
}


DigitalOut led(13);
SquareWave oscillator(1.0);

rational01_t x = 0;

void begin() {
///  led.on();
}

void step() {
  x+=RATIONAL_MAX/1000;
//  println(x/(float)(RATIONAL_MAX));
//  println(rational_to_float(x));
  println( rational_sin32(x) );
//  oscillator >> led;
}