#include "benchmarks/basicmath/snipmath.h"
#include "cmsis_os.h"
#include "ImC/imc_kernel.h"
#include "ImC/imc_extension.h"
// #include "imc.h"

#include <stdio.h>

void vBasicMath()
{
  double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
  double a2 = 1.0, b2 = -4.5, c2 = 17.0, d2 = -30.0;
  double a3 = 1.0, b3 = -3.5, c3 = 22.0, d3 = -31.0;
  double a4 = 1.0, b4 = -13.7, c4 = 1.0, d4 = -35.0;
  double x[3];
  double X;
  int solutions;
  int i;
  unsigned long l = 0x3fed0169L;
  struct int_sqrt q;

  double sum = 0;

  // portDISABLE_INTERRUPTS();
  // printf("Sum: %f\r\n", sum);
  // portENABLE_INTERRUPTS();

  // while (1)
  // {
  // int IMC_REPEAT = 2;
  int IMC_REPEAT = imcBENCH_REPEAT_COUNT;
  for(int imc_repeat=0; imc_repeat<IMC_REPEAT; imc_repeat++) {
    sum = 0;

    /* Now solve some random equations */
    for (a1 = 1; a1 < 3; a1++)
    {
      // imcREQUEST_CHECKPOINT();
      for (b1 = 10; b1 > 0; b1--)
      {
        IMC_UNROLL(2)
        for (c1 = 5; c1 < 15; c1 += 0.5)
        {
          IMC_UNROLL(10)
          for (d1 = -1; d1 > -11; d1--)
          {
            SolveCubic(a1, b1, c1, d1, &solutions, x);
            sum += x[0];
            // portDISABLE_INTERRUPTS();
            // printf("Solutions:");
            // portENABLE_INTERRUPTS();
            // for (i = 0; i < solutions; i++)
            // {
            //   portDISABLE_INTERRUPTS();
            //   printf(" %f", x[i]);
            //   portENABLE_INTERRUPTS();
            // }
            // portDISABLE_INTERRUPTS();
            // printf("\r\n");
            // portENABLE_INTERRUPTS();
            // for(int j=0; j<1000000; j++) { __asm__("  nop"); }
          }
        }
      }
    }
  }

  portDISABLE_INTERRUPTS();
  // printf("Sum: %f\r\n", sum);
  printf("(OUT) Sum: %d\r\n", (int)sum);
  portENABLE_INTERRUPTS();

  // #if( imcDEBUG_ITM_MESSAGES == 1 )
  //   imcLOG_BENCHMARK_END();
  // #endif
  // exit(0);

  // }
}