// This code takes a cartesian coordinate, [x,y,z], and converts it to
// a spherical coordinate [azimuth, elevation, radius] ... and back again.
// If the roundtrip error is greater than a threshold, it is reported.

// The goal is to minimize the error while keeping the run-time overhead in
// the spherical -> cartesian stage as low as possible.  (This is the part
// that will run on the 8-bit Vectrex. We don't care about the run-time of
// the cartesian -> spherical conversion, which is only ever done once, and
// that on a fast 32-bit linux system).

// Once we have bidirectional spherical<->cartesian conversion working, I can
// create models based on spherical coordinates, and convert those spherical
// coordinates within the game to cartesian coordinates, which will be
// projected to screen coordinates using the code in 'percy.c'.

// Objects defined by spherical coordinates can be easily rotated on the ground
// plane by a simple 8-bit modulo addition to their azimuth value, and tilted
// up or down by increasing or decreasing their elevation value.  As long as
// those are the only motions needed by the game (which is often the case) and
// not some complex motion such as rolling around their axis of travel, then
// we should have a very cheap way of animating 3D models.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "types.h"
#include "demo-model.h"

static int debug = 0;

#define PI 3.1415926

#define ACCURACY 11  // An error of 10 is accepted.  An error > 10 will be reported.
                     // Note that the error rate is higher the finer the resolution
                     // of the model coordinates.  If the models coordinates are snapped
                     // to multiples of 16, the error doesn't exceed 4.

int errors[ACCURACY];

/*

With the current code, the vast majority of positioning errors will be 6 units or fewer off
from desired.  Plus 1 unit for perspective approximation.  Possibly too inaccurate for
static images but quite reasonable for fast-moving objects in a video game.

0: 9034
1: 432282
2: 2225512
3: 4184565
4: 4456438  <-- mode
5: 3182801
6: 1608614
7: 544920
8: 118684
9: 13566
10: 786
 */

#define FAKEFP8 int8_t // Big explanation of this below.  You need to undertand this fully if working on this code.

float angle256_to_radians(u_int8_t angle256) {
  // 2 * PI radians = 1 circle
  float tmp = ((float)angle256 / 256.0) * (PI*2.0);
  if (debug) fprintf(stderr, "angle256_to_radians(%d) -> %f\n", angle256, tmp);
  return tmp;
}

int radians_to_angle256(float radians) {
  // 2 * PI radians = 1 circle
  int tmp = (int)((radians * 256.0) / (PI*2.0));
  if (debug) fprintf(stderr, "radians_to_angle256(%f) -> %d\n", radians, tmp);
  return tmp;
}

int8_t trigf_to_trig256(float t) { // scale a float so results can be used in integer CPU
  if (t <= -0.999 || t >= 0.999) {
    // parameters should already have been tweaked to 127/128 of original value.
    fprintf(stderr, "trigf_to_trig256: rangecheck!  t = %f\n", t);
  }
  int tmp = roundf(t*128.0); // -1.0:0.999 -> -128:127
  
  if (debug) fprintf(stderr, "trigf_to_trig256(%f) -> %d", t, tmp);
  if (debug) fprintf(stderr, "\n");
  return (int8_t)tmp;
}

// sin256 and cos256 will only ever be called when undoing spherical coordinates back to
// cartesian coordinates on the Vectrex.  The result of the sin256 and cost256 functions
// is a sort of 0.8 fixed point, *BUT* in the range -127:127 rather than -128:128 which
// would have required 2-byte variables to store for the want of 1 in 256 values :-(

FAKEFP8 sin256(u_int8_t angle256) {
  if (debug) fprintf(stderr, ">> sin256(%d)\n", angle256);
  float radians = angle256_to_radians(angle256);
  if (debug) fprintf(stderr, "  sin256: angle256 = %d -> %f radians\n", angle256, radians);
  float f_sin = (sin(radians)*127.0)/128.0;                                    // ****** <--- RESCALING DOWN TO -127:127 PSEUDO-FIXED-POINT
  if (debug) fprintf(stderr, "  sin256: sin(%f) -> %f\n", radians, f_sin);
  int tmp = trigf_to_trig256(f_sin);
  if (debug) fprintf(stderr, "  sin256: trigf_to_trig256(%f) -> %d\n", f_sin, tmp);
  if (debug) fprintf(stderr, "<< sin256(%d) -> %d\n", angle256, tmp);

  if (tmp >=  128) {
    fprintf(stderr, "sin256: Assertion failed: value %d >= 128\n", tmp);
    tmp =  127;
  }
  if (tmp <= -128) {
    fprintf(stderr, "sin256: Assertion failed: value %d <= -128\n", tmp);
    tmp = -127;
  }
  
  return tmp;
}

// *** RESCALE FROM -1:1 TO -127/128:127/128.  Only cos and sin values need to be constrained to be in the -127:127 range
FAKEFP8 cos256(u_int8_t angle256) { // cos256 and sin256 will be replaced on Vectrex by a lookup table.
  if (debug) fprintf(stderr, ">> cos256(%d)\n", angle256);
  float radians = angle256_to_radians(angle256);
  if (debug) fprintf(stderr, "  cos256: angle256 = %d -> %f radians\n", angle256, radians);
  float f_cos = (cos(radians)*127.0)/128.0;                                    // ****** <--- RESCALING DOWN TO -127:127 PSEUDO-FIXED-POINT
  if (debug) fprintf(stderr, "  cos256: cos(%f) -> %f\n", radians, f_cos);
  int tmp = trigf_to_trig256(f_cos);
  if (debug) fprintf(stderr, "  cos256: trigf_to_trig256(%f) -> %d\n", f_cos, tmp);
  if (debug) fprintf(stderr, "<< cos256(%d) -> %d\n", angle256, tmp);

  if (tmp >=  128) {
    fprintf(stderr, "cos256: Assertion failed: value %d >= 128\n", tmp);
    tmp =  127;
  }
  if (tmp <= -128) {
    fprintf(stderr, "cos256: Assertion failed: value %d <= -128\n", tmp);
    tmp = -127;
  }
  
  return tmp;
}

int acos256(float anglef) { // only used on linux code to build initial model.  Not needed on Vectrex side
  float f_acos;
  u_int8_t i_acos;
  
  if (debug) fprintf(stderr, "acos256(%f)\n", anglef);
  // angle should be in the range -1.0:1.0

  if (anglef < -1.0 || anglef > 1.0) {
    fprintf(stderr, "acos256: rangecheck!  anglef = %f\n", anglef);
  }

  f_acos = acosf(anglef); // Note: no need to do range compression here.  Only on Vectrex side.
  i_acos = radians_to_angle256(f_acos);
  
  if (debug) fprintf(stderr, "acosf(%f) -> %f -> %d", anglef, f_acos, i_acos);
  if (debug) fprintf(stderr, "\n");
  
  return i_acos;
}

int atan256(float x, float y) { // only used on linux code to build initial model.  Not needed on Vectrex side
  float tmp = atan2f(x, y);
  if (debug) fprintf(stderr, "atan256(%f,%f) -> %f -> %d",
          x, y, tmp,
          radians_to_angle256(tmp));
  if (debug) fprintf(stderr, "\n");
  return radians_to_angle256(tmp);
}

// parameters must be pre-adjusted to be relative to center of the model
// (or the preferred point for rotating around, for instance in Tokyo Drift,
// if you set the rotation point to be closer to the front of the vehicle,
// the back-end swings around just the way you would want it to at no extra
// run-time cost! :-) )  However if using a non-centered rotation point, we
// may need to be careful that rotated large models don't place any points
// outside the 256x256x256 model space...

void spherical(int x, int y, int z, u_int8_t *az, u_int8_t *el, u_int8_t *dist) {
  // Only ever used on Linux for generating initial model.  Not needed on Vectrex.
  
  // The horizontal plane (eg of battlezone) is actually left/right -> x
  // and near/far -> z.  (Up/down -> y).  So azimuth is f(x,z) and elevation is
  // f(azimuth,y)...
  
  /* viewed from above the ground plane of Battlezone:

                          z
                          ^
                          |
                         (x,?,z point)
                          .
                          |\
                          | \
                          |  \
                          |   \
                          |  az\
                          +-----o (0,0,0 origin)   ---> x

      viewed from the eye into the screen:

                   y
                   ^
                   |
                   (x,y,? point)
                    _______
                   |      /
                   |     /
                   |    / 
                   |   /  
                   |el/   
                   | /
                   |/
   (0,0,0 origin)  o  ---> x

   */
  if (debug) fprintf(stderr, ">> spherical(%d,%d,%d)\n",x,y,z);
  float r = sqrtf(x*x+y*y+z*z);
  *dist = (u_int8_t)roundf(r);
  *az = atan256(z,x);

  *el = acos256(y/r);
  if (debug) fprintf(stderr, "<< spherical(%d,%d,%d) -> %u,%u,%u\n",x,y,z, *az, *el, *dist);
}


void cartesian(u_int8_t az, u_int8_t el, u_int8_t r, int8_t *x, int8_t *y, int8_t *z) {
  // Vectrex code.
  
  if (debug) fprintf(stderr, ">> cartesian(%u,%u,%u)\n", az, el, r);
  int X0, Y0, Z0, X, Y, Z;

  // On vectrex, right-shifts of 16-bit product can be avoided by accessing
  // high and low bytes of 16-bit product separately via a struct...
  // Five 8x8 -> 16-bit multiplies (MUL instruction) cannot be avoided.

  // BE VERY CAREFUL WITH THIS CODE!  The results of sin256 and cos256 are
  // 8-bit 'FAKEFP8' pseudo-fixed-point variables in the range -127:127, which
  // must be taken into account when performing arithmetic with them!
  
  X0 = ( r * sin256(el))/254;   // should all these be >>7 ?  NORMALIZING THE PRODUCT OF A TRUE INTEGER WITH A FAKEFP8 ACTUALLY NEEDS A /254 NOT A /256 (>>8)
  X  = (X0 * cos256(az))/254;   //                            SO AMOUNT OF INACCURACY IS CREATED HERE.
  
  Z0 = ( r * sin256(el))/254;   //                            FOR TESTING *ONLY* WE WILL DO THE /254 AND COMPARE ACCURACY RESULTS!
  Z  = (Z0 * sin256(az))/254;

  // hack for 2X here?
  Y = (r * cos256(el))/127; // >>7;  // these more expensive divides are only during development testing.  They'll be gone in the Vectrex-specific code.
  if (Y >=  128) { // WORLD COORDINATE TEST
    //fprintf(stderr, "cartesian: Assertion #3 failed: value %d >= 128\n", Y);
    Y =  127;
  }
  if (Y < -128) { // WORLD COORDINATE TEST
    fprintf(stderr, "cartesian: Assertion #4 failed: value %d <= -128\n", Y);
    Y = -127;
  }
  
  if (debug) fprintf(stderr, "  cartesian_x: r=%d * sin(el=%d)=%d * cos(az=%d)=%d -> %d\n",
          r,
          el, sin256(el),
          az, cos256(az),
          X);
  if (debug) fprintf(stderr, "  cartesian_y: r=%d * sin(el=%d)=%d * sin(az=%d)=%d -> %d\n",
          r,
          el, sin256(el),
          az, sin256(az),
          X);

  if (debug) fprintf(stderr, "<< cartesian(%u,%u,%u) -> %d,%d,%d\n", az, el, r, X, Y, Z);

  // I put in these <<2 tweaks from inspection, I'm not sure where they are
  // coming from.  My best guess is that there's a multiply or divide by 128
  // somewhere that should have been by 256, or vice versa.  This is separate
  // from the issue of rescaling the fixed-point sin and cos to use the range
  // -127/128:127/128 rather than -1:1 which was *extremely* necessary to
  // keep the majority of the calculations using 8-bit variables.
  X = X<<2;
  if (X >=  128) { // WORLD COORDINATE TEST
    //fprintf(stderr, "cartesian: Assertion #5 failed: value %d >= 128\n", X);
    X =  127;
  }
  if (X < -128) { // WORLD COORDINATE TEST
    fprintf(stderr, "cartesian: Assertion #6 failed: value %d <= -128\n", X);
    X = -127;
  }
  
  Z = ((Z<<2)*127)>>7;
  if (Z >=  128) { // WORLD COORDINATE TEST
    fprintf(stderr, "cartesian: Assertion #7 failed: value %d >= 128\n", Z);
    Z =  127;
  }
  if (Z < -128) { // WORLD COORDINATE TEST
    //fprintf(stderr, "cartesian: Assertion #8 failed: value %d <= -128\n", Z);
    Z = -127;
  }
  
  *x = X; *y = Y; *z = Z;
}

int between(int low, int mid, int high) {
  if (low <= mid && mid <= high) return TRUE;
  if (high <= mid && mid <= low) return TRUE;
  return FALSE;
}

int main(int argc, char **argv) {
  int8_t nx, ny, nz;
  u_int8_t az, el, r;


  // positive x axis is angle 0
  
  // elevation of 0 is straight up, elevation of 128 is straight down  
  // [x=0,y= 100,z=0] -> [az=0, el=  0, r=100]
  // [x=0,y=-100,z=0] -> [az=0, el=128, r=100]
    
  // azimuth of 0 is to the right (x = 100) , azimuth of 128 is to the left (x = -100)
  // [x= 100,y=0,z=0] -> [az=  0, el=64, r=100]
  // [x=-100,y=0,z=0] -> [az=128, el=64, r=100]

  // from right to right front increases az from 0 to 32, so azimuth increases anti-clockwise.
  // [x=100,y=  0,z=0] -> [az=0, el=64, r=100]
  // [x=100,y=100,z=0] -> [az=0, el=32, r=141]
  // moving vertically up 100 from 100 right of center changes elevation
  // from 63 to 32, so elevation 0 is straight up and increases as it lowers to 128 straight down
  // ***NOTE** since full range is not used, we *could* scale elevation by 2 for more accuracy
  // when converting back to cartesian...
  
  for (int x = -128; x < 128; x += 1) {
    for (int y = -128; y < 128; y += 1) {
      for (int z = -128; z < 128; z += 1) {
        spherical(x, y, z, &az, &el, &r);
        cartesian(az, el, r, &nx, &ny, &nz);

        // can filter output when debugging by redirecting stdout and stderr separately...
        fprintf(stdout, "[x=%d,y=%d,z=%d] -> [az=%d, el=%d, r=%d]", x, y, z, az, el, r);
        fprintf(stdout, " -> [%d, %d, %d]", nx, ny, nz);
        if (!(   between(x-ACCURACY, nx, x+ACCURACY)
              && between(y-ACCURACY, ny, y+ACCURACY)
              && between(z-ACCURACY, nz, z+ACCURACY))) {
          fprintf(stdout, " *** Warning: round-trip inaccuracy > %d", ACCURACY);
          fprintf(stderr, "Warning: round-trip inaccuracy > %d  ([%d,%d,%d] -> [%d, %d, %d])", ACCURACY, x, y, z, nx, ny, nz);
        }
        fprintf(stdout, "\n");
        int err, worst_err = 0;
        err = nx-x; if (err < 0) err = -err;
        if (err > worst_err) worst_err = err;
        err = ny-y; if (err < 0) err = -err;
        if (err > worst_err) worst_err = err;
        err = nz-z; if (err < 0) err = -err;
        if (err > worst_err) worst_err = err;
        if (worst_err < ACCURACY) errors[worst_err] += 1;
      }
    }
  }

  int i;
  for (i = 0; i < ACCURACY; i++) {
    fprintf(stderr, "%d: %d\n", i, errors[i]);
  }

  int p;
  fprintf(stderr, "\n// coordinates from demo-model.h converted to spherical:\n");
  fprintf(stderr, "static const Vertex init_spherical[DEMO_POINTS] = {\n");
  for (p = 0; p < DEMO_POINTS; p++) {
    spherical(init_cartesian[p].coord[0], init_cartesian[p].coord[1], init_cartesian[p].coord[2], &az, &el, &r);
    fprintf(stderr, "  /* %2d */ {{ %d, %d, %d }},\n", p, az, el, r);
  }
  fprintf(stderr, "};\n\n");
  
  exit(EXIT_SUCCESS);
  return EXIT_FAILURE;
}