#include <Arduino.h>

// Function to encode a 32-bit float into a 16-bit float16
uint16_t encodeToFloat16(float value) {
    uint32_t floatBits = *((uint32_t*)&value); // Interpret the float as a 32-bit integer
    uint32_t sign = (floatBits >> 31) & 0x1;  // Extract the sign bit
    int32_t exponent = ((floatBits >> 23) & 0xFF) - 127 + 15; // Adjust exponent bias
    uint32_t mantissa = floatBits & 0x7FFFFF; // Extract the mantissa (23 bits)

    // Handle special cases
    if (exponent <= 0) {
        // Value is too small for float16; return zero
        return sign << 15;
    } else if (exponent > 31) {
        // Value is too large for float16; return infinity
        return (sign << 15) | (0x1F << 10);
    }

    // Round the mantissa to fit into 10 bits
    mantissa = mantissa + 0xFFF + ((mantissa >> 13) & 1); // Round to nearest, ties to even
    mantissa = mantissa >> 13;

    // Handle rounding overflow
    if (mantissa & 0x400) {
        mantissa = 0;
        exponent++;
    }

    // Pack the float16 representation
    return (sign << 15) | ((exponent & 0x1F) << 10) | (mantissa & 0x3FF);
}

// Function to decode a 16-bit float16 back into a 32-bit float
float decodeFromFloat16(uint16_t encodedValue) {
    uint32_t sign = (encodedValue >> 15) & 0x1;  // Extract the sign bit
    int32_t exponent = ((encodedValue >> 10) & 0x1F) - 15 + 127; // Adjust exponent bias
    uint32_t mantissa = encodedValue & 0x3FF;   // Extract the mantissa (10 bits)

    // Handle special cases
    if (exponent <= 0) {
        // Subnormal or zero
        if (mantissa == 0) {
            // Zero
            return sign ? -0.0f : 0.0f;
        } else {
            // Subnormal (denormalized number)
            return (sign ? -1.0f : 1.0f) * mantissa * pow(2, -24);
        }
    } else if (exponent >= 255) {
        // Infinity or NaN
        return sign ? -INFINITY : INFINITY;
    }

    // Pack the float32 representation
    uint32_t floatBits = (sign << 31) | ((exponent & 0xFF) << 23) | (mantissa << 13);
    return *((float*)&floatBits); // Interpret the bits as a float
}

void setup() {
    // Initialize Serial Monitor
    Serial.begin(9600);
    while (!Serial) { ; } // Wait for Serial Monitor to initialize

    // Example float value
    float originalValue = 655.05;

    // Encode to float16
    uint16_t encodedValue = encodeToFloat16(originalValue);

    // Decode back to float
    float decodedValue = decodeFromFloat16(encodedValue);

    // Print results
    Serial.println("Original Float Value: " + String(originalValue, 5));
    Serial.println("Encoded Value: " + String(encodedValue));
    Serial.println("Encoded Value: 0x" + String(encodedValue, HEX));
    Serial.println("Decoded Float Value: " + String(decodedValue, 5));
}

void loop() {
    // No action required in the loop
}