#include <stdint.h>
#include <stdbool.h>
#include <math.h>

#define N 10

#define ZFA


int32_t emulate_fcvtmod_w_d(double a) {
    // 1. Trap NaNs and Infinities (Spec says they must return 0)
    if (!isfinite(a)) {
        return 0;
    }

    // 2. Bring the double into a range that fits in a 64-bit int.
    // We use fmod to strip away everything above 2^32.
    // (2^32 is exactly 4294967296.0)
    double truncated_range = fmod(a, 4294967296.0);

    // 3. Convert to 64-bit integer with truncation (RTZ)
    int64_t standard_int = (int64_t)truncated_range;

    // 4. Cast to 32-bit signed integer (effectively your SLLI/SRAI)
    return (int32_t)standard_int;
}

static int baremetal_errno = 0;
int *__errno(void) {
    return &baremetal_errno;
}

static inline uint64_t read_cycles() {
  uint64_t start;
  asm volatile ("rdcycle %0" : "=r"(start));
  return start;
}

int main() {
  // -- fround.s --
  {
    volatile float a, b;
    read_cycles();
    for (int i = 0; i < N; i++) {
      a = 3.25f;
      b = round(a);
    }
    read_cycles();
  }

  // -- fround.d --
  {
    volatile double a, b;
    read_cycles();
    for (int i = 0; i < N; i++) {
      a = 3.25;
      b = round(a);
    }
    read_cycles();
  }

  // -- fleq.s --
  {
    volatile float a = 3.25f, b = 3.0f;
    int res;
    read_cycles();
    for (int i = 0; i < N; i++) {
#ifndef ZFA
      asm volatile (
        "fclass.s t0, %1\n\t"
        "fclass.s t1, %2\n\t"
        "or       t0, t0, t1\n\t"
        "andi     t2, t0, 0x200\n\t"
        "bnez     t2, 1f\n\t"
        "fle.s    %0, %1, %2\n\t"
        "j        2f\n\t"
        "1:\n\t"
        "li       %0, 0\n\t"
        "2:\n\t"
        : "=r" (res)
        : "f" (a), "f" (b)
        : "t0", "t1", "t2"
      );
#else
      asm volatile ("fleq.s %0, %1, %2" : "=r" (res) : "f" (a), "f" (b));
#endif
    }
    read_cycles();
  }

  // -- fleq.d --
  {
    volatile double a = 3.25, b = 3.0;
    int res;
    read_cycles();
    for (int i = 0; i < N; i++) {
#ifndef ZFA
      asm volatile (
        "fclass.d t0, %1\n\t"
        "fclass.d t1, %2\n\t"
        "or       t0, t0, t1\n\t"
        "andi     t2, t0, 0x200\n\t"
        "bnez     t2, 1f\n\t"
        "fle.d    %0, %1, %2\n\t"
        "j        2f\n\t"
        "1:\n\t"
        "li       %0, 0\n\t"
        "2:\n\t"
        : "=r" (res)
        : "f" (a), "f" (b)
        : "t0", "t1", "t2"
      );
#else
      asm volatile ("fleq.d %0, %1, %2" : "=r" (res) : "f" (a), "f" (b));
#endif
    }
    read_cycles();
  }

  // -- fminm.s --
  {
    volatile float a = 0.0f, b = -0.0f, res;
    read_cycles();
    for (int i = 0; i < N; i++) {
#ifndef ZFA
      asm volatile (
        "fclass.s t0, %1\n\t"
        "fclass.s t1, %2\n\t"
        "li       t2, 0x300\n\t"
        "and      t3, t0, t2\n\t"
        "and      t4, t1, t2\n\t"
        "bnez     t3, 1f\n\t"
        "bnez     t4, 2f\n\t"
        "fmin.s   %0, %1, %2\n\t"
        "j        3f\n\t"
        "1:\n\t"
        "bnez     t4, 4f\n\t"
        "fmv.s    %0, %2\n\t"
        "j        3f\n\t"
        "2:\n\t"
        "fmv.s    %0, %1\n\t"
        "j        3f\n\t"
        "4:\n\t"
        "fmin.s   %0, %1, %2\n\t"
        "3:\n\t"
        : "=f" (res)
        : "f" (a), "f" (b)
        : "t0", "t1", "t2", "t3", "t4"
      );
#else
      asm volatile ("fminm.s %0, %1, %2" : "=f" (res) : "f" (a), "f" (b));
#endif
    }
    read_cycles();
  }

  // -- fminm.d --
  {
    volatile double a = 0.0, b = -0.0, res;
    read_cycles();
    for (int i = 0; i < N; i++) {
#ifndef ZFA
      asm volatile (
        "fclass.d t0, %1\n\t"
        "fclass.d t1, %2\n\t"
        "li       t2, 0x300\n\t"
        "and      t3, t0, t2\n\t"
        "and      t4, t1, t2\n\t"
        "bnez     t3, 1f\n\t"
        "bnez     t4, 2f\n\t"
        "fmin.d   %0, %1, %2\n\t"
        "j        3f\n\t"
        "1:\n\t"
        "bnez     t4, 4f\n\t"
        "fmv.d    %0, %2\n\t"
        "j        3f\n\t"
        "2:\n\t"
        "fmv.d    %0, %1\n\t"
        "j        3f\n\t"
        "4:\n\t"
        "fmin.d   %0, %1, %2\n\t"
        "3:\n\t"
        : "=f" (res)
        : "f" (a), "f" (b)
        : "t0", "t1", "t2", "t3", "t4"
      );
#else
      asm volatile ("fminm.d %0, %1, %2" : "=f" (res) : "f" (a), "f" (b));
#endif
    }
    read_cycles();
  }

  // -- fli.s --
  {
    volatile float res[32];
    read_cycles();
    for (int i = 0; i < N; i++) {
      res[0]  = -1.0f;
      res[1]  = -1.0f;
      res[2]  = 0x1p-16f;
      res[3]  = 0x1p-15f;
      res[4]  = 0x1p-14f;
      res[5]  = 0x1p-13f;
      res[6]  = 0x1p-12f;
      res[7]  = 0x1p-11f;
      res[8]  = 0x1p-10f;
      res[9]  = 0x1p-9f;
      res[10] = 0x1p-8f;
      res[11] = 0x1p-7f;
      res[12] = 0x1p-6f;
      res[13] = 0x1p-5f;
      res[14] = 0x1p-4f;
      res[15] = 0x1p-3f;
      res[16] = 0.25f;
      res[17] = 0.5f;
      res[18] = 0.75f;
      res[19] = 1.0f;
      res[20] = 1.25f;
      res[21] = 1.5f;
      res[22] = 1.75f;
      res[23] = 2.0f;
      res[24] = 2.5f;
      res[25] = 3.0f;
      res[26] = 4.0f;
      res[27] = 8.0f;
      res[28] = 16.0f;
      res[29] = 32.0f;
      res[30] = INFINITY;
      res[31] = NAN;
    }
    read_cycles();
  }

  // -- fli.d --

  {
    volatile double res[32];
    read_cycles();
    for (int i = 0; i < N; i++) {
      res[0]  = -1.0;
      res[1]  = -1.0;
      res[2]  = 0x1p-16;
      res[3]  = 0x1p-15;
      res[4]  = 0x1p-14;
      res[5]  = 0x1p-13;
      res[6]  = 0x1p-12;
      res[7]  = 0x1p-11;
      res[8]  = 0x1p-10;
      res[9]  = 0x1p-9;
      res[10] = 0x1p-8;
      res[11] = 0x1p-7;
      res[12] = 0x1p-6;
      res[13] = 0x1p-5;
      res[14] = 0x1p-4;
      res[15] = 0x1p-3;
      res[16] = 0.25;
      res[17] = 0.5;
      res[18] = 0.75;
      res[19] = 1.0;
      res[20] = 1.25;
      res[21] = 1.5;
      res[22] = 1.75;
      res[23] = 2.0;
      res[24] = 2.5;
      res[25] = 3.0;
      res[26] = 4.0;
      res[27] = 8.0;
      res[28] = 16.0;
      res[29] = 32.0;
      res[30] = INFINITY;
      res[31] = NAN;
    }
    read_cycles();
  }

  // -- fcvtmod.w.d --
  {
    static const double inputs[N] = {
      2147483649.0,   //  2^31 + 1   -> -2147483647 (mod 2^32)
     -2147483649.0,   // -2^31 - 1   ->  2147483647 (mod 2^32)
      4294967297.0,   //  2^32 + 1   ->          1 (mod 2^32)
     -4294967297.0,   // -2^32 - 1   ->         -1 (mod 2^32)
      3.25,           //  in-range
      0.0,            //  exact zero
     -0.0,            //  negative zero
      INFINITY,
     -INFINITY,
      NAN
    };
    volatile double a;
    int32_t res;
    read_cycles();
    for (int i = 0; i < N; i++) {
      a = inputs[i];
    asm volatile("" : "+f"(a));
#ifndef ZFA
      res = emulate_fcvtmod_w_d(a);
#else
      asm volatile ("fcvtmod.w.d %0, %1, rtz" : "=r" (res) : "f" (a));
#endif
  asm volatile("" : : "r"(res));
    }
    read_cycles();
  }

  return 0;
}