#include <stdio.h>
#include <math.h>
#include <stdint.h>
#include <stdlib.h>

typedef uint32_t u32;

// have to a multiply of 2
#define COMMA 8
#define COMMA_FIX (COMMA >> 1)

void heron_sqrt(u32 *mem) {
  int i = mem[0];
  while (i > 0) {
    u32 s = mem[i];
    u32 x = s;
    if (s > 1) {
      // x_0 = (s + 1) / 2
      // without increment to avoid overflow for 0xffffffff
      x = x >> 1;
      u32 old_x = x;

      while(1) {
        // x_{n + 1} = (x_n + (s / x_n)) / 2
        x = (x >> 1) + ((s/x) >> 1);
        if (old_x <= x) {
          break;
        }
        old_x = x;
      }
    }
    mem[i] = x << COMMA_FIX;
    i--;
  }
}

int main() {
  float data[] = {
    0,
    //0.5,
    1.0 / (1 << COMMA),
    1.0 / (1 << (COMMA - 1)),
    1,
    2,
    2.5,
    2.25,
    2.125,
    2.0625,
    2.03125, // precision test
    3<<2,
    1<<3,
    1<<4,
    1<<5,
    1<<6,
    5<<2,
    11<<7,
    1<<7,
    1<<8,
    1<<16,
    1<<17,
    1<<18,
    1<<19,
    1<<20, // 32 - 14 = 20
    1<<24, // 32 -  8 = 24
    1<<30,
    0xffffffff
  };
  const int length = sizeof(data)/sizeof(float);
  int i;

  u32* mem = malloc((length + 1) * sizeof(u32));
  mem[0] = length;

  for (i = 1; i <= length; i++) {
    // float to fixed point
    mem[i] = (u32) (data[i] * (1 << COMMA));
  }

  heron_sqrt(mem);

  for (i = 0; i < length; i++) {
    // out-range-check
    if (data[i] <= (0xffffffff >> COMMA)) {
      printf("s             = %10.10f\n", data[i]);
      printf("sqrt(s)       = %10.10f\n", sqrt(data[i]));
      // fixed point to float
      printf("heron_sqrt(s) = %10.10f\n", ((float) mem[i]) / (1 << COMMA));
    }
    puts("\n");
  }
  return 0;
}