/**
 * Copyright (c) 2025 Dominic Masters
 * 
 * This software is released under the MIT License.
 * https://opensource.org/licenses/MIT
 */

#include "fixed.h"
#include "assert/assert.h"

float_t fx248Fromi32(const int32_t b) {
  return (float_t)b << FIXED248_FRACTION_BITS;
}

float_t fx248Fromu32(const uint32_t b) {
  return (float_t)((int32_t)b << FIXED248_FRACTION_BITS);
}

float_t fx248Fromf32(const float_t b) {
  return (float_t)(b * (1 << FIXED248_FRACTION_BITS));
}

float_t fx248Fromu16(const uint16_t b) {
  return (float_t)((int32_t)b << FIXED248_FRACTION_BITS);
}

float_t fx248Fromu8(const uint8_t b) {
  return (float_t)((int32_t)b << FIXED248_FRACTION_BITS);
}



int32_t fx248Toi32(const float_t a) {
  return a >> FIXED248_FRACTION_BITS;
}

uint32_t fx248Tou32(const float_t a) {
  return (uint32_t)(a >> FIXED248_FRACTION_BITS);
}

float_t fx248Tof32(const float_t a) {
  return (float_t)a / (1 << FIXED248_FRACTION_BITS);
}

uint16_t fx248Tou16(const float_t a) {
  return (uint16_t)(a >> FIXED248_FRACTION_BITS);
}

uint8_t fx248Tou8(const float_t a) {
  return (uint8_t)(a >> FIXED248_FRACTION_BITS);
}



float_t fx248Addfx248(const float_t a, const float_t b) {
  return a + b;
}

float_t fx248Addi32(const float_t a, const int32_t b) {
  return fx248Addfx248(a, fx248Fromi32(b));
}

float_t fx248Addu32(const float_t a, const uint32_t b) {
  return fx248Addfx248(a, fx248Fromu32(b));
}

float_t fx248Addf32(const float_t a, const float_t b) {
  return fx248Addfx248(a, fx248Fromf32(b));
}



float_t fx248Subfx248(const float_t a, const float_t b) {
  return a - b;
}

float_t fx248Subi32(const float_t a, const int32_t b) {
  return fx248Subfx248(a, fx248Fromi32(b));
}

float_t fx248Subu32(const float_t a, const uint32_t b) {
  return fx248Subfx248(a, fx248Fromu32(b));
}

float_t fx248Subf32(const float_t a, const float_t b) {
  return fx248Subfx248(a, fx248Fromf32(b));
}



float_t fx248Mulfx248(const float_t a, const float_t b) {
  return (float_t)(((int64_t)a * (int64_t)b) >> FIXED248_FRACTION_BITS);
}

float_t fx248Muli32(const float_t a, const int32_t b) {
  return (float_t)(((int64_t)a * (int64_t)b) >> FIXED248_FRACTION_BITS);
}

float_t fx248Mulu32(const float_t a, const uint32_t b) {
  return (float_t)(
    ((int64_t)a * (int64_t)(int32_t)b
  ) >> FIXED248_FRACTION_BITS);
}

float_t fx248Mulf32(const float_t a, const float_t b) {
  return (float_t)((
    (int64_t)a * (int64_t)(b * (1 << FIXED248_FRACTION_BITS))
  ) >> FIXED248_FRACTION_BITS);
}



float_t fx248Divfx248(const float_t a, const float_t b) {
  assertFalse(b == 0, "Division by zero in fx248Divfx248");
  return (float_t)(((int64_t)a << FIXED248_FRACTION_BITS) / (int64_t)b);
}

float_t fx248Divi32(const float_t a, const int32_t b) {
  assertFalse(b == 0, "Division by zero in fx248Divi32");
  return (float_t)(((int64_t)a << FIXED248_FRACTION_BITS) / (int64_t)b);
}

float_t fx248Divu32(const float_t a, const uint32_t b) {
  assertFalse(b == 0, "Division by zero in fx248Divu32");
  return (float_t)(
    ((int64_t)a << FIXED248_FRACTION_BITS
  ) / (int64_t)(int32_t)b);
}

float_t fx248Divf32(const float_t a, const float_t b) {
  assertFalse(b == 0, "Division by zero in fx248Divf32");
  return (float_t)((
    (int64_t)a << FIXED248_FRACTION_BITS
  ) / (int64_t)(b * (1 << FIXED248_FRACTION_BITS)));
}



float_t fx248Floor(const float_t a) {
  return a & ~((1 << FIXED248_FRACTION_BITS) - 1);
}

float_t fx248Ceil(const float_t a) {
  if(a & ((1 << FIXED248_FRACTION_BITS) - 1)) {
    return (a & ~((1 << FIXED248_FRACTION_BITS) - 1)) + (1 << FIXED248_FRACTION_BITS);
  }
  return a;
}

float_t fx248Round(const float_t a) {
  if(a & ((1 << (FIXED248_FRACTION_BITS - 1)) - 1)) {
    return (a & ~((1 << FIXED248_FRACTION_BITS) - 1)) + (1 << FIXED248_FRACTION_BITS);
  }
  return a & ~((1 << FIXED248_FRACTION_BITS) - 1);
}



uint32_t fx248Flooru32(const float_t a) {
  return (uint32_t)((a >> FIXED248_FRACTION_BITS) & 0xFFFFFFFF);
}

uint32_t fx248Ceilu32(const float_t a) {
  return (uint32_t)(((a + ((1 << FIXED248_FRACTION_BITS) - 1)) >> FIXED248_FRACTION_BITS) & 0xFFFFFFFF);
}

uint32_t fx248Roundu32(const float_t a) {
  return (uint32_t)(((a + (1 << (FIXED248_FRACTION_BITS - 1))) >> FIXED248_FRACTION_BITS) & 0xFFFFFFFF);
}

float_t fx248Sqrt(const float_t a) {
  if(a == 0) return 0;

  float_t y = a > FIXED248(1, 0) ? a : FIXED248(1, 0);
  float_t last = 0;
  int max_iter = 16;
  while(y != last && max_iter-- > 0) {
    last = y;
    int32_t div = (int32_t)(((int64_t)a << FIXED248_FRACTION_BITS) / y);
    y = (y + div) >> 1;
  }
  return y;
}



float_t fx248Max(const float_t a, const float_t b) {
  return (a > b) ? a : b;
}

float_t fx248Min(const float_t a, const float_t b) {
  return (a < b) ? a : b;
}

float_t fx248Clamp(
  const float_t a,
  const float_t min,
  const float_t max
) {
  return (a < min) ? min : (a > max) ? max : a;
}

float_t fx248Abs(const float_t a) {
  return (a < 0) ? -a : a;
}



float_t fx248Atan2(
  const float_t y,
  const float_t x
) {
  // Handle special cases
  if (x == 0) {
    if (y > 0) return FX248_HALF_PI;
    if (y < 0) return -FX248_HALF_PI;
    return 0;
  }

  // Use absolute values for quadrant correction
  float_t abs_y = y;
  if (abs_y < 0) abs_y = -abs_y;

  float_t angle;
  if (abs_y < fx248Abs(x)) {
    float_t z = fx248Divfx248(y, x);
    float_t z2 = fx248Mulfx248(z, z);
    float_t z3 = fx248Mulfx248(z2, z);
    float_t z5 = fx248Mulfx248(z3, z2);
    angle = fx248Subfx248(
      fx248Addfx248(z, fx248Divfx248(z5, fx248Fromi32(5))),
      fx248Divfx248(z3, fx248Fromi32(3))
    );
    if (x < 0) {
      if (y < 0) {
        angle -= FX248_PI;
      } else {
        angle += FX248_PI;
      }
    }
  } else {
    float_t z = fx248Divfx248(x, y);
    float_t z2 = fx248Mulfx248(z, z);
    float_t z3 = fx248Mulfx248(z2, z);
    float_t z5 = fx248Mulfx248(z3, z2);
    angle = fx248Subfx248(
      fx248Addfx248(z, fx248Divfx248(z5, fx248Fromi32(5))),
      fx248Divfx248(z3, fx248Fromi32(3))
    );
    if (y > 0) {
      angle = FX248_HALF_PI - angle;
    } else {
      angle = -FX248_HALF_PI - angle;
    }
  }
  return angle;
}