#include <stdint.h>
#include "operations.h"

#define FRACTION_BITS 8 // Define the number of fractional bits
#define PI_FIXED ((int16_t)(3.14159265358979323846 * (1 << 15)))

typedef int16_t fixed_point; // Define the fixed-point integer type

fixed_point float_to_fixed(float x) {
    return (int16_t)(x * (1 << FRACTION_BITS)); // Convert float to fixed-point integer
}

float fixed_to_float(fixed_point x) {
    return (float)x / (1 << FRACTION_BITS); // Convert fixed-point integer to float
}

fixed_point fixed_add(fixed_point x, fixed_point y) {
    return x + y; // Addition of fixed-point integers
}

fixed_point fixed_subtract(fixed_point x, fixed_point y) {
    return x - y; // Subtraction of fixed-point integers
}

fixed_point fixed_multiply(fixed_point x, fixed_point y) {
    return (int16_t)(((int32_t)x * y) >> FRACTION_BITS); // Multiplication of fixed-point integers
}

fixed_point fixed_divide(fixed_point x, fixed_point y) {
    return (int16_t)(((int32_t)x << FRACTION_BITS) / y); // Division of fixed-point integers
}

complex_fixed complex_float_to_fixed(float real, float imag) {
    complex_fixed result;
    result.real = float_to_fixed(real);
    result.imag = float_to_fixed(imag);
    return result;
}

complex_fixed complex_fixed_add(complex_fixed x, complex_fixed y) {
    complex_fixed result;
    result.real = fixed_add(x.real, y.real);
    result.imag = fixed_add(x.imag, y.imag);
    return result;
}

complex_fixed complex_fixed_subtract(complex_fixed x, complex_fixed y) {
    complex_fixed result;
    result.real = fixed_subtract(x.real, y.real);
    result.imag = fixed_subtract(x.imag, y.imag);
    return result;
}

complex_fixed complex_fixed_multiply(complex_fixed x, complex_fixed y) {
    complex_fixed result;
    result.real = fixed_subtract(fixed_multiply(x.real, y.real), fixed_multiply(x.imag, y.imag));
    result.imag = fixed_add(fixed_multiply(x.real, y.imag), fixed_multiply(x.imag, y.real));
    return result;
}

complex_fixed complex_fixed_divide(complex_fixed x, complex_fixed y) {
    complex_fixed result;
    fixed_point denom = fixed_add(fixed_multiply(y.real, y.real), fixed_multiply(y.imag, y.imag));
    result.real = fixed_divide(fixed_add(fixed_multiply(x.real, y.real), fixed_multiply(x.imag, y.imag)), denom);
    result.imag = fixed_divide(fixed_subtract(fixed_multiply(x.imag, y.real), fixed_multiply(x.real, y.imag)), denom);
    return result;
}

int16_t atan2_fixed(int16_t y, int16_t x) {
    int16_t quotient = 0;
    int16_t remainder = 0;

    if (x == 0) {
        if (y > 0) {
            return PI_FIXED/2;
        } else if (y < 0) {
            return -PI_FIXED/2;
        } else {
            return 0;
        }
    }

    quotient = (int16_t)(((int32_t)y << 15) / x);

    if (y == 0) {
        if (x > 0) {
            return 0;
        } else {
            return PI_FIXED;
        }
    } else if (y > 0) {
        if (x > 0) {
            remainder = (int16_t)(((int32_t)y << 15) - ((int32_t)x * quotient));
            return (int16_t)(atan_fixed(quotient, remainder) * (1 << 15));
        } else {
            remainder = (int16_t)(((int32_t)y << 15) - ((int32_t)x * quotient));
            return (int16_t)((atan_fixed(quotient, remainder) + PI_FIXED) * (1 << 15));
        }
    } else {
        if (x > 0) {
            remainder = (int16_t)(((int32_t)y << 15) - ((int32_t)x * quotient));
            return (int16_t)(atan_fixed(quotient, remainder) * (1 << 15));
        } else {
            remainder = (int16_t)(((int32_t)y << 15) - ((int32_t)x * quotient));
            return (int16_t)((atan_fixed(quotient, remainder) - PI_FIXED) * (1 << 15));
        }
    }
}

int16_t atan_fixed(int16_t quotient, int16_t remainder) {
    int32_t x2 = (int32_t)quotient * quotient;
    int32_t a = x2 * 15708;    // 15708 = 4 * 65536 * tan(pi/4)
    int32_t b = ((int32_t)536870912 + a) >> 30; // 536870912 = 2^29
    int16_t c = (int16_t)(((int32_t)quotient << 14) / ((int32_t)536870912 - b));
    int16_t d = (int16_t)(2 * c / ((int32_t)1 + x2 * (int32_t)c * c / ((int32_t)1 << 28)));
    return (int16_t)(((int32_t)d * (int32_t)remainder) / (1 << 15));
}

int16_t hypot_fixed(int16_t x, int16_t y) {
    double x_f = (double)x / 32767.0;
    double y_f = (double)y /32767.0;;
    double r = (x_f * x_f) + (y_f * y_f);
    double hypot_f = sqrt(r);
    int16_t hypot_i = (int16_t)round(hypot_f * 32767.0);
    return hypot_i;
}