#include <stdio.h>
#include <math.h>
#include "atan_result.h"
#include "atan_param_x.h"
#include "atan_param_z.h"

// This code is taken from https://geekshavefeelings.com/posts/fixed-point-atan2
// I've taken that code and shoe-horned it in to an interface that returns values
// that are very close to the values that Scratch returns for the same parameters
// - at least when used in the context of this program!  I make no claims at all
// for it working in any other context.

// In scratch, the parameters are floats, whereas here they are short ints, so
// there's a small loss of precision there; and the results here are short ints
// too, so there's a second loss of precision.  Despite that, the results are
// pretty close (usually less than 2 units error) and definitely good enough
// for a video game.

// I leave most of the original comments intact.

/*
 * fxpt_atan2.c
 *
 * Copyright (C) 2012, Xo Wang
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is furnished to do
 * so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

/**
 * Negative absolute value. Used to avoid undefined behavior for most negative
 * integer (see C99 standard 7.20.6.1.2 and footnote 265 for the description of
 * abs/labs/llabs behavior).
 *
 * @param i 16-bit signed integer
 * @return negative absolute value of i; defined for all values of i
 */
static inline short s16_nabs(const short j) {
#if (((short)-1) >> 1) == ((short)-1)
  // signed right shift sign-extends (arithmetic)
  const short negSign = ~(j >> 15); // splat sign bit into all 16 and complement
  // if j is positive (negSign is -1), xor will invert j and sub will add 1
  // otherwise j is unchanged
  return (j ^ negSign) - negSign;
#else
  return (j < 0 ? j : -j);
#endif
}

unsigned short muldiv(unsigned short a, unsigned short b, unsigned short c)
{
#ifdef VECTREX_CMOC
  unsigned short q = 0, r = 0, qn, rn;
  // from https://stackoverflow.com/questions/4144232/how-to-calculate-a-times-b-divided-by-c-only-using-32-bit-integer-types-even-i
  // ... doing it the hard way, without double precision of any kind, either in C or assembler...
  if (a < b) {
    qn = b / c;
    rn = b % c;
    while (a) {
      if (a&1) { q += qn; r += rn; if (r >= c) { q++; r -= c; } }
      a >>= 1; qn <<= 1; rn <<= 1; if (rn >= c) { qn++; rn -= c; }
    }
  } else {
    // the while loop should be over the smaller variable, for speed
    qn = a / c;
    rn = a % c;
    while (b) {
      if (b&1) { q += qn; r += rn; if (r >= c) { q++; r -= c; } }
      b >>= 1; qn <<= 1; rn <<= 1; if (rn >= c) { qn++; rn -= c; }
    }
  }
 return q; // also, r = (a * b) % c   (q = quotient, r = remainder)
#else
 return (unsigned short)(((unsigned long)a * (unsigned long)b) / (unsigned long)c);
#endif
}

static inline short q15_mul(const short j, const short k) {
  // I'll fix this later.
  if (j<0 && k<0) return (short)muldiv(-j,-k,1<<15);
  if (j<0) return -(short)muldiv(-j,k,1<<15);
  if (k<0) return -(short)muldiv(j,-k,1<<15);
  return (short)muldiv(j, k, 1<<15);
}

static inline short q15_div(const short numer, const short denom) {
  // Quick hack.  It works.
  if (numer<0 && denom<0) return (short)muldiv(-numer, 1<<15,  -denom);
  if (numer<0) return -(short)muldiv(-numer, 1<<15,  denom);
  if (denom<0) return -(short)muldiv(numer, 1<<15,  -denom);
  return (short)muldiv(numer, 1<<15,  denom);
}

/**
 * 16-bit fixed point four-quadrant arctangent. Given some Cartesian vector
 * (x, y), find the angle subtended by the vector and the positive x-axis.
 *
 * The value returned is in units of 1/65536ths of one turn. This allows the use
 * of the full 16-bit unsigned range to represent a turn. e.g. 0x0000 is 0
 * radians, 0x8000 is pi radians, and 0xFFFF is (65535 / 32768) * pi radians.
 *
 * Because the magnitude of the input vector does not change the angle it
 * represents, the inputs can be in any signed 16-bit fixed-point format.
 *
 * @param y y-coordinate in signed 16-bit
 * @param x x-coordinate in signed 16-bit
 * @return angle in (val / 32768) * pi radian increments from 0x0000 to 0xFFFF
 */
unsigned short fxpt_atan2(const short y, const short x) {
  if (x == y) { // x/y or y/x would return -1 since 1 isn't representable
    if (y > 0) { // 1/8
      return 8192;
    } else if (y < 0) { // 5/8
      return 40960;
    } else { // x = y = 0
      return 0;
    }
  }
  const short nabs_y = s16_nabs(y), nabs_x = s16_nabs(x);
  if (nabs_x < nabs_y) { // octants 1, 4, 5, 8
    const short y_over_x = q15_div(y, x);
    const short correction = q15_mul(/*q15_from_double(0.273 * M_1_PI)*/2847, s16_nabs(y_over_x));
    const short unrotated = q15_mul(/*q15_from_double(0.25 + 0.273 * M_1_PI)*/11039 + correction, y_over_x);
    if (x > 0) { // octants 1, 8
      return unrotated;
    } else { // octants 4, 5
      return 32768 + unrotated;
    }
  } else { // octants 2, 3, 6, 7
    const short x_over_y = q15_div(x, y);
    const short correction = q15_mul(/*q15_from_double(0.273 * M_1_PI)*/2847, s16_nabs(x_over_y));
    const short unrotated = q15_mul(/*q15_from_double(0.25 + 0.273 * M_1_PI)*/11039 + correction, x_over_y);
    if (y > 0) { // octants 2, 3
      return 16384 - unrotated;
    } else { // octants 6, 7
      return 49152 - unrotated;
    }
  }
}

int Scratch_atan2(int x, int y)  // substitute for atan2(x/y)
{
  // sign shenannigans needed to match Scratch's behaviour...
  int result = ((fxpt_atan2(x, y)>>8)-128)*360/256;

  if (result <= -90) {
    return result + 180;
  } else if (result >= 90) {
    return result - 180;
  }
  return result;

}

int Scratch_atan2_fakedegrees(int x, int y)  // substitute for atan2(x/y) but using 256 angles in a circle rather than 360 degrees
{
  int result = ((fxpt_atan2(x, y)>>8)-128);

  if (result <= -64) {
    return result + 128;
  } else if (result >= 64) {
    return result - 128;
  }
  return result;

}

int main(int argc, char **argv)
{
  int i;
  short x, z;
  for (i = 0; i < 1733; i++) {
    x = (short)lrint(atan_x[i]); z = (short)lrint(atan_z[i]);
    fprintf(stderr, "atan2(%f,%f) = %hd (%hd)\n", atan_x[i], atan_z[i], Scratch_atan2(x,z), (short)lrint(atan_result[i]));
  }
  return 0;
}