//      (SEE FILE tg.c IN THIS DIRECTORY FOR THE VECTREX GRAPHICS OUTPUT.)

/*

   After many previous attempts, I am having one final effort to create a C version
   of Tailgunner!  (and so far I'm delighted to report that it is going well...)

   This source file contains only the linux-based code for precomputing the flight paths
   of attacking enemy ships.  Yhose flight paths are written out as a C header file and
   used as data by the Vectrex code in tg.c to display the graphics in real time.

   This source file has support for the 3.5inch SPI display which uses
   a framebuffer driver (it copies whatever is written to the framebuffer over to
   the LCD at regular intervals, but not fast enough (with the current driver) for
   full 30Hz updates.)  It is used for debugging the flight path generation.

     On my Pi Zero with an HDMI display enabled and the 3.5 inch LCD on SPI being used for
     the game, you need to use the map:01 option in cmdline.txt:
     console=serial0,115200 console=tty1 root=PARTUUID=9cb46872-02 rootfstype=ext4 fsck.repair=yes rootwait cfg80211.ieee80211_regdom=US fbcon=map:01

   To reduce the runtime overhead in a Vectrex implementation of Tailgunner,
   we are copying the style of the original arcade version by pre-computing
   the flight paths.

   Coordinate space is X:-512:511 Y:-512:511 Z:0:1023  Viewer is at [0,0,0]

   Tailgunner ships initially fly over a 2D plane placed at maximum distance
   from the player (Z=1023).  At some point they then turn out of that plane
   and fly towards the viewer, having reoriented the ship so that it is
   facing us, with the ship's 'up' being the world's 'up'.  The ships at
   this stage only ever approach us (decreasing Z) but can turn up/down and
   left/right.  Generally the left/right displacement is in one direction
   only, but they can weave up or down at random.

   This code now implements the transition from the XY plane to the XZ/YZ planes.
   It does so by causing them to fly around a quarter circle placed on a sphere
   starting at the back of the sphere and ending on one of the edges where Z is
   the center of the sphere so that when the ship departs from the sphere at a
   tangent, it is flying directly at the viewer/player.  We also implement a
   'straighten up and fly right' procedure to smoothly restore head-up orientation
   after a ship has made a sharp turn.

   Other support code such as interpolating between different orientations, and
   generating flight paths using 3D Bezier curves is also implemented but may
   not all be in use at any specific point during development.

   The data that are saved to the arrays in the Vectrex rom are only the screen
   X and Y positions of the center of the ship, a scale factor for the ship to
   be drawn at using hardware scaling, and the rx,ry,rz rotations needed to draw
   the ship at the correct rotational orientation.  The Vectrex does not need to
   know the true 3D universe positions of the ship.  Missile targetting is not
   true 3D, it is primarily 2D with a fudge factor for the distance to the
   center of the ship based on the scale value.
   
   We may also want to take into account the viewing angle from the viewer to
   the ship when working out the rotation to use when rendering the ship.  This
   will use quaternions in the same way that the ship rotations in flight do.

*/

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

#include "linux_graphics.h"

// Data for saving to header file for vectrex drawing code:
uint8_t save_rx[256], save_ry[256], save_rz[256]; // rotation vector x,y,z
int8_t save_sx[256], save_sy[256]; // screen x,y
uint8_t save_sc[256]; // scale

typedef struct Vector { // sometimes known as gl::vec3 ...
  float x, y, z;
} Vector;

typedef struct {
    float w, x, y, z;
} Quaternion;

#define GENPATH 1 // object_t slightly different for the two programs
#include "drawship.c"

// float degree256 = 2.0*M_PI/256.0;

static int iround(float f) {
  if (f < 0.0) return -(int)(-f+0.5); // round away from zero so converted extremes are -128 and 128.
  return (int)(f+0.5);
}

static inline Vector cross(Vector v1, Vector v2){
   static Vector r;
   r.x = ((v1.y * v2.z) - (v1.z * v2.y));
   r.y = ((v1.z * v2.x) - (v1.x * v2.z));
   r.z = ((v1.x * v2.y) - (v1.y * v2.x));
   return r;
}

static inline float dot(Vector a, Vector b) {
  return a.x*b.x + a.y*b.y + a.z*b.z;
}

static inline Vector normalize(Vector v) {
  static Vector r;
  float len = sqrtf(dot(v, v));
  r.x = v.x/len; r.y = v.y/len; r.z = v.z/len;
  return r;
}

static inline Vector add(Vector a, Vector b) {
  a.x += b.x; a.y += b.y; a.z += b.z;
  return a;
}

static inline Vector sub(Vector a, Vector b) {
  a.x -= b.x; a.y -= b.y; a.z -= b.z;
  return a;
}

static inline Vector rescale(Vector v, float s) {
  v.x *= s; v.y *= s; v.z *= s;
  return v;
}

static object_t *ship[4] = { &model1, &model2, &model3, &model4 };

//static inline void save_frame(int Frame,
//                              const object_t *object,
//                              uint8_t rx, uint8_t ry, uint8_t rz,
//                              uint8_t sc,
//                              int8_t sx, int8_t sy) {
//  // SAVE PARAMETER FOR OUTPUT TO VECTREX HEADER FILE
//  save_sx[Frame] = sx; save_sy[Frame] = sy;
//  save_sc[Frame] = sc;
//}

// Convert the orientation as defined by the forward and up vectors to an Euler
// rotation rx,ry,rz performed in that order as per the Vectrex display code.
void get_rotation_parameters(Vector forward, Vector up, float *rx, float *ry, float *rz) {
    Vector f = normalize(forward);
    Vector u = normalize(up);
    Vector r = cross(u, f);
    r = normalize(r);
    u = cross(f, r);  // be sure that the up vector is consistent...
    // - this is paranoia and can probably be removed for speed, after checking.

    *ry = asinf(-f.x);
    if (fabsf(cosf(*ry)) > 1e-6) {
        *rx = atan2f(f.y, f.z);
        *rz = atan2f(u.x, r.x);
    } else {
        // Gimbal lock
        *rx = 0.0f;
        *rz = atan2f(-r.y, u.y);
    }
}

// (when I came up with this arc algorithm I think I unwittingly reinvented slerp -
//  page 55 of https://cseweb.ucsd.edu/classes/wi20/cse169-a/slides/CSE169_03.pdf )


// Function to output the trajectory of the ship as it performs the manoeuver out of
// the rear 2D Z plane and into the upright-oriented approach to the player/viewer:

void manoeuver_arc(object_t ship, int N, float R) {
    // Set sphere center C such that the ship is touching the farthest point on the sphere
    float Cx = ship.location.x;
    float Cy = ship.location.y;
    float Cz = ship.location.z-R;

    // Vector from starting point A to center of sphere
    float ax = ship.location.x - Cx;
    float ay = ship.location.y - Cy;
    float az = ship.location.z - Cz;

    // Rotation axis is normalized 2D forward vector projected on XY plane
    Vector axis = normalize((Vector){ ship.forward.x, ship.forward.y, 0.0f });

    // A great circle around the sphere that touches the ship will have a
    // point 90 degrees round the circle that is created by on the circumference of
    // the sphere when projected in X,Y as viewed by the player.  If the
    // ship tangentially departs from the arc across the sphere at this point,
    // it will be heading directly to the viewer, with decreasing Z.
    float theta_total = M_PI * 0.5f;
    float dtheta = theta_total / N;

    // Loop over N intermediate steps, a step per frame, so choose N such that
    // the turn out of the rear Z plane takes an appropriate length of time.
    // Initial guess being about half a second (which I now think may be too much)
    for (int i = 1; i <= N; i += 1) {
        float theta = i * dtheta;

        // Rodrigues' rotation formula to rotate vector A around axis by angle theta
        Vector v = { ax, ay, az };
        Vector k = axis;

        float cos_t = cosf(theta);
        float sin_t = sinf(theta);

        // maybe neater to call cross9) and dot() below.  They're inline procdures anyway.
        float k_dot_v = k.x * v.x + k.y * v.y + k.z * v.z;

        // Cross product k x v
        Vector k_cross_v = {
            k.y * v.z - k.z * v.y,
            k.z * v.x - k.x * v.z,
            k.x * v.y - k.y * v.x
        };

        // Rodrigues rotated vector p = v*cos theta + (k x v)*sin theta + k*(k . v)*(1 - cos theta)
        Vector p = {
            v.x * cos_t + k_cross_v.x * sin_t + k.x * k_dot_v * (1 - cos_t),
            v.y * cos_t + k_cross_v.y * sin_t + k.y * k_dot_v * (1 - cos_t),
            v.z * cos_t + k_cross_v.z * sin_t + k.z * k_dot_v * (1 - cos_t)
        };

        // Compute ship position at this step
        float x = Cx + p.x;
        float y = Cy + p.y;
        float z = Cz + p.z;

        // Tangent at point p is axis x radius vector p
        Vector tangent = {
            axis.y * p.z - axis.z * p.y,
            axis.z * p.x - axis.x * p.z,
            axis.x * p.y - axis.y * p.x
        };

        // Normalize tangent to get current 'forward' vector
        Vector forward = normalize(tangent);

        // 'up' vector points toward circle center from current position
        Vector up = normalize(sub((Vector){ Cx, Cy, Cz }, (Vector){ x, y, z }));

        // Output the position and orientation vectors for this step
        printf("Step %2d: Pos(%8.3f, %8.3f, %8.3f) Forward(%6.3f, %6.3f, %6.3f) Up(%6.3f, %6.3f, %6.3f)\r\n",
               i, x, y, z, forward.x, forward.y, forward.z, up.x, up.y, up.z);

        // TO DO: write parameters to array or output file or just draw the ship
        // for feedback while testing by updating the ship's object_t struct
    }

    // TO DO: After finishing the quarter-circle arc, add code to right the ship's 'up' vector 
    // as it approaches the viewer (facing along -Z).
}


// (See pp 463-465 of https://mathweb.ucsd.edu/~sbuss/MathCG2/SecondEdDraft.pdf )


// Create quaternion from rotation matrix components
Quaternion matrix_to_quat(float m00, float m01, float m02,
                          float m10, float m11, float m12,
                          float m20, float m21, float m22) {
    Quaternion q;
    float trace = m00 + m11 + m22;
    if (trace > 0.0f) {
        float s = sqrtf(trace + 1.0f) * 2.0f;
        q.w = 0.25f * s;
        q.x = (m21 - m12) / s;
        q.y = (m02 - m20) / s;
        q.z = (m10 - m01) / s;
    } else if ((m00 > m11) && (m00 > m22)) {
        float s = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
        q.w = (m21 - m12) / s;
        q.x = 0.25f * s;
        q.y = (m01 + m10) / s;
        q.z = (m02 + m20) / s;
    } else if (m11 > m22) {
        float s = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
        q.w = (m02 - m20) / s;
        q.x = (m01 + m10) / s;
        q.y = 0.25f * s;
        q.z = (m12 + m21) / s;
    } else {
        float s = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
        q.w = (m10 - m01) / s;
        q.x = (m02 + m20) / s;
        q.y = (m12 + m21) / s;
        q.z = 0.25f * s;
    }
    return q;
}

// Convert forward and up vectors to a unit quaternion
Quaternion forward_up_to_quat(Vector forward, Vector up) {
    forward = normalize(forward);
    up = normalize(up);
    
    // Compute right vector as cross product of up and forward
    Vector right = normalize(cross(up, forward));
    
    // Recompute orthonormal up vector to ensure orthogonality
    up = cross(forward, right);
    
    // Construct rotation matrix columns: right, up, forward
    return matrix_to_quat( // column-major
        right.x, up.x, forward.x,
        right.y, up.y, forward.y,
        right.z, up.z, forward.z
    );
}

Quaternion quat_from_rotation_around_x(float angle) {
  // Convert a rotation around the X axis (at the origin of the model) to a rotation specified by a Quaternion.
  float s = sinf(angle / 2.0f);
  float c = cosf(angle / 2.0f);
  return (Quaternion){c, s, 0.0f, 0.0f};
}

Quaternion quat_from_rotation_around_y(float angle) {
  // Convert a rotation around the Y axis (at the origin of the model) to a rotation specified by a Quaternion.
  float s = sinf(angle / 2.0f);
  float c = cosf(angle / 2.0f);
  return (Quaternion){c, 0.0f, s, 0.0f};
}

Quaternion quat_from_rotation_around_z(float angle) {
  // Convert a rotation around the Z axis (at the origin of the model) to a rotation specified by a Quaternion.
  float s = sinf(angle / 2.0f);
  float c = cosf(angle / 2.0f);
  return (Quaternion){c, 0.0f, 0.0f, s};
}

// #################################################################################

// Multiply two quaternions: result = QOrientation * QRotation
Quaternion quat_multiply(Quaternion QOrientation, Quaternion QRotation) {
    Quaternion result;
    result.w = QOrientation.w * QRotation.w - QOrientation.x * QRotation.x - QOrientation.y * QRotation.y - QOrientation.z * QRotation.z;
    result.x = QOrientation.w * QRotation.x + QOrientation.x * QRotation.w + QOrientation.y * QRotation.z - QOrientation.z * QRotation.y;
    result.y = QOrientation.w * QRotation.y - QOrientation.x * QRotation.z + QOrientation.y * QRotation.w + QOrientation.z * QRotation.x;
    result.z = QOrientation.w * QRotation.z + QOrientation.x * QRotation.y - QOrientation.y * QRotation.x + QOrientation.z * QRotation.w;
    return result;
}

// Convert quaternion to forward and up vectors
void quat_to_forward_up(Quaternion q, Vector *forward, Vector *up) {
    // From the quaternion, reconstruct the rotation matrix columns
    // Forward vector corresponds to the third column (Z axis)
    forward->x = 2.0f * (q.x * q.z + q.w * q.y);
    forward->y = 2.0f * (q.y * q.z - q.w * q.x);
    forward->z = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
    *forward = normalize(*forward);

    // Up vector corresponds to the second column (Y axis)
    up->x = 2.0f * (q.x * q.y - q.w * q.z);
    up->y = 1.0f - 2.0f * (q.x * q.x + q.z * q.z);
    up->z = 2.0f * (q.y * q.z + q.w * q.x);
    *up = normalize(*up);
}

// TO DO: We *might* add a yaw/pitch rotation over and above the ones representing the
// model's orientation, in order to compensate for the slight difference in viewing
// angle when the ship is to the left or right and above or below the viewer's eye
// at [0,0,0].  The horizontal components of the viewing angle will be tan(ship.x/ship.z)
// or something like that, with tan(ship.y/ship.z) for the vertical angle.  Again, an
// expensive calculation, but we don't care because it's all pre-computed offline.

void pitch(float angle, Vector *forward, Vector *up) {
  Quaternion QOrientation = forward_up_to_quat(*forward, *up);
  Quaternion QRotation = quat_from_rotation_around_x(angle);
  Quaternion Updated_orientation = quat_multiply(QRotation, QOrientation);  
  quat_to_forward_up(Updated_orientation, forward, up);
}

void yaw(float angle, Vector *forward, Vector *up) {
  Quaternion QOrientation = forward_up_to_quat(*forward, *up);
  Quaternion QRotation = quat_from_rotation_around_y(angle);
  Quaternion Updated_orientation = quat_multiply(QRotation, QOrientation);
  quat_to_forward_up(Updated_orientation, forward, up);
}

void roll(float angle, Vector *forward, Vector *up) {
  Quaternion QOrientation = forward_up_to_quat(*forward, *up);
  Quaternion QRotation = quat_from_rotation_around_z(angle);
  Quaternion Updated_orientation = quat_multiply(QRotation, QOrientation);
  quat_to_forward_up(Updated_orientation, forward, up);
}

// #####################################################################################

float quat_dot(Quaternion q1, Quaternion q2) {
  return q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z;
}

void quat_normalize(Quaternion *q) {
  float norm = sqrtf(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
  if (norm > 0.0f) {
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;
  }
}

Quaternion quat_slerp(Quaternion q1, Quaternion q2, float t) {
  // Compute angle between quats
  float dot = quat_dot(q1, q2);
  // If dot < 0, negate one to take shortest path
  if (dot < 0.0f) {
    q2.w = -q2.w; q2.x = -q2.x; q2.y = -q2.y; q2.z = -q2.z;
    dot = -dot;
  }
  const float DOT_THRESHOLD = 0.9995f;
  if (dot > DOT_THRESHOLD) { // Linear interpolation if very close
    Quaternion result;
    result.w = q1.w + t*(q2.w - q1.w);
    result.x = q1.x + t*(q2.x - q1.x);
    result.y = q1.y + t*(q2.y - q1.y);
    result.z = q1.z + t*(q2.z - q1.z);
    quat_normalize(&result);
    return result;
  }
  // SLERP
  float theta_0 = acosf(dot); // angle between q1 and q2
  float theta = theta_0 * t;
  float sin_theta = sinf(theta);
  float sin_theta_0 = sinf(theta_0);

  float s1 = cosf(theta) - dot * sin_theta / sin_theta_0;
  float s2 = sin_theta / sin_theta_0;

  Quaternion result;
  result.w = q1.w * s1 + q2.w * s2;
  result.x = q1.x * s1 + q2.x * s2;
  result.y = q1.y * s1 + q2.y * s2;
  result.z = q1.z * s1 + q2.z * s2;
  quat_normalize(&result);
  return result;
}

void handle_vstep(Vector forward, Vector up, int i) {
  // Convert from quaternion to forward/up form and add orientation vector to
  // interpolated path using 3D Bezier curve
  printf("Step %2d: Forward(%6.3f, %6.3f, %6.3f) Up(%6.3f, %6.3f, %6.3f)\r\n",
         i,
         forward.x, forward.y, forward.z,
         up.x, up.y, up.z);
}

void handle_qstep(Quaternion q, int i) {
  Vector forward, up;
  // Convert from quaternion to forward/up form and add orientation vector to
  // interpolated path using 3D Bezier curve
  quat_to_forward_up(q, &forward, &up);
  handle_vstep(forward, up, i);
}

// Main interpolation functions
void interpolate_quats(Quaternion q1, Quaternion q2, int steps) {
  if (steps < 2) return; // nothing to interpolate
  handle_qstep(q1, 0);
  for (int i = 1; i < steps; i += 1) {
    float t = (float)i / (float)steps;
    Quaternion qi = quat_slerp(q1, q2, t);
    handle_qstep(qi, i);
  }
  handle_qstep(q2, steps);
}

void interpolate_vectors(Vector v1forward, Vector v1up, Vector v2forward, Vector v2up, int steps) {
  Quaternion q1 = forward_up_to_quat(v1forward, v1up);
  Quaternion q2 = forward_up_to_quat(v2forward, v2up);
  interpolate_quats(q1, q2, steps);
}

// #####################################################################################

// This section is to cause a rotated ship to right itself into a more expected
// orientation, aften turning out from the rear 2D plane and heading towards the
// player.  However the code I'm using to make a turn look natural will leave the
// ship at an angle when it comes out of the turn, so this code will right the ship.

static Vector slerp(Vector v0, Vector v1, float t) {
  v0 = normalize(v0);
  v1 = normalize(v1);

  float dotp = dot(v0, v1);
  if (dotp > 1.0f) dotp = 1.0f;
  if (dotp < -1.0f) dotp = -1.0f;

  float theta = acosf(dotp) * t;
  Vector relative_vec = sub(v1, rescale(v0, dotp));
  relative_vec = normalize(relative_vec);

  Vector part1 = rescale(v0, cosf(theta));
  Vector part2 = rescale(relative_vec, sinf(theta));

  return add(part1, part2);
}

void set_orientation(Vector *forward, Vector *up, Vector target_forward, Vector target_up, int steps) {
  for (int step = 0; step <= steps; step++) {
    float t = (float)step / (float)steps;
    Vector f_interp = slerp(*forward, target_forward, t);
    Vector u_interp = slerp(*up, target_up, t);

    // Normalize results
    f_interp = normalize(f_interp);
    u_interp = normalize(u_interp);

    // Update forward and up for next step
    if (step < steps) {
      *forward = f_interp;
      *up = u_interp;
      printf("Pos(%8.3f, %8.3f, %8.3f) Forward(%6.3f, %6.3f, %6.3f) Up(%6.3f, %6.3f, %6.3f)\r\n",
             ship[0]->location.x, ship[0]->location.y, ship[0]->location.z,
             ship[0]->forward.x, ship[0]->forward.y, ship[0]->forward.z,
             ship[0]->up.x, ship[0]->up.y, ship[0]->up.z);
      // TO DO: move and redraw the ship after each step (when debugging)
    }
  }
}

// Single-dimensional component of a bezier between two points.  Call separately for each
// dimension, call repeatedly for multiple segments.  Path starts along vector AB and
// ends along vector CD.  This is point 't' in a Path of 2^logmaxslots.
// Delta is filled in with a vector roughly tangential to the path at this point,
// which is used along with function 'direction' to point the ship in the direction
// of travel, since the vector from point [t-1] to [t+1] is likely very short and
// therefore the angle will suffer badly from rounding error.

// See http://stackoverflow.com/questions/5443653/opengl-coordinates-from-bezier-curves
// for the diagram explaining AB/CD.
int d1bezier(int A,  // Start value
             int B,  // First control value
             int C,  // Second control value
             int D,  // Ending value
             int t,  // Time slice, 0..t..511
             int logmaxslots, // 9 for 512
             int *delta) { // updated with X, Y, or Z component of tangent for 'forward' vector
  int s = ((1<<logmaxslots)-1) - t;
  int AB = (A*s + B*t)>>logmaxslots;
  int BC = (B*s + C*t)>>logmaxslots;
  int CD = (C*s + D*t)>>logmaxslots;
  int ABC = (AB*s + CD*t)>>logmaxslots;
  int BCD = (BC*s + CD*t)>>logmaxslots;
  *delta = ABC - BCD; // update tangent component in this axis for 'forward' vector.
                      // I don't think this scheme can give us the 'up' or 'right' components however. 
  return (ABC*s + BCD*t)>>logmaxslots;
}

/*

  *** NOTE ***  Scaling code at the moment is pretty much a random placeholder and
                generates pretty crazy screen locations.  Needs to actually have
                some thought applied.  Especially with respect to the view fulcrum
                because ships can go out of view but still be in the universe cube.
                Flight pathing needs to try to restrict movements to be in the
                viewable area.

                So DO NOT TRUST the three procedures below.


  The perspective code in the old implementation was something like this...
  
#define TWEAK 2048
xa = ((TWEAK*tx0) / (TWEAK+tz0));
ya = ((TWEAK*ty0) / (TWEAK+tz0));
xb = ((TWEAK*tx1) / (TWEAK+tz1));
yb = ((TWEAK*ty1) / (TWEAK+tz1));
//#undef TWEAK

// scale by depth  ">>10" is short for "/ZDEPTH"
xa = (xa*(z+200))>>10; ya=(ya*(z+200))>>10; xb=(xb*(z+200))>>10; yb=(yb*(z+200))>>10;
 */

// Used to determine the scale at which to draw the ship:
int Scale_fn(float z) {
  // The idea is roughly that a line of 128 world units at z=0 will be drawn as say 20 units
  // at z=1023.

  // revz == 0 -> 20
  // revz == 1023 -> 256
  
  int sc = (int)((float)((1023.0-z)/1023.0) * (256.0-20.0)  + 20.0);
  if (sc > 254) sc = 254;
  if (sc < 20) sc = 20;
  fprintf(stderr, "SCALE: 1023-z=%f -> scale = %d\r\n", 1023-z, sc);
  return sc;
}

// These two are used to position the center of the ship on the screen
int project_x(float x, float z) {
  // trying a weird fulcrum compression where everything is stretched to -128:128 regardless of distance.
  // It's not sensible but I'ld like to see how it looks...
  
  // x in the range -512:512 should map to the screen's x of say -40:40 at z=1023 but -128:128 at z=0
  return (int) ( (x/512.0) * 128.0  );
  //             -1:1
}

int project_y(float y, float z) {
  return (int) ( (y/512.0) * 128.0  );
}

void Output_data(int Frame, object_t *ship, Vector right, float rx, float ry, float rz) {
    fprintf(stderr, "Frame:    %d  (t=%.2fs)\r\n", Frame, (float)Frame/(float)FPS);
    fprintf(stderr, "Forward:  [%d %d %d]  (x %0.3f y %0.3f z %0.3f)  ",
            iround(ship->forward.x*128.0), iround(ship->forward.y*128.0), iround(ship->forward.z*128.0),
            ship->forward.x, ship->forward.y, ship->forward.z);
    fprintf(stderr, "Up: [%d %d %d]  (x %0.3f y %0.3f z %0.3f)  ",
            iround(ship->up.x*128.0), iround(ship->up.y*128.0), iround(ship->up.z*128.0),
            ship->up.x, ship->up.y, ship->up.z);
    fprintf(stderr, "{Right: [%d %d %d]  (x %0.3f y %0.3f z %0.3f)}\r\n",
            iround(right.x*128.0), iround(right.y*128.0), iround(right.z*128.0),
            right.x, right.y, right.z);
    fprintf(stderr, "Rotation: %d %d %d  (%f %f %f)\r\n",
            iround(rx*128.0/M_PI), iround(ry*128.0/M_PI), iround(rz*128.0/M_PI),
            rx, ry, rz);
    fprintf(stderr, "Screen:   [%d,%d] at scale %d\r\n", ship->x, ship->y, Scale_fn(ship->location.z));
    fprintf(stderr, "Location: [%d %d %d]  ([%f %f %f])\r\n\n",
            iround(ship->location.x), iround(ship->location.y), iround(ship->location.z),
            ship->location.x, ship->location.y, ship->location.z);
}


#define ANGLE_UNITS (2.0*M_PI/256.0)

void flightpath0(int Frame, object_t *ship) {
  if (Frame == 0) {
    // Initial and final ship positions for each flight path to be selected semi at random,
    // consistent with the arcade game.
    ship->location.x = 0;
    ship->location.y = 0;
    ship->location.z = 1023;
    ship->speed = 4.0;
    yaw(128*ANGLE_UNITS, &ship->forward, &ship->up); // turn around to face us
  } else {
    // apply movement calculated on last frame.  (forward vector holds delta x,y,z.)
    ship->location.x += ship->forward.x*ship->speed;
    ship->location.y += ship->forward.y*ship->speed;
    ship->location.z += ship->forward.z*ship->speed;
  }
}

void flightpath1(int Frame, object_t *ship) {
  if (Frame == 0) {
    // Initial and final ship positions for each flight path to be selected semi at random,
    // consistent with the arcade game.
    ship->location.x = 0;
    ship->location.y = 0;
    ship->location.z = 1023;
    ship->speed = 4.0;
    pitch(64*ANGLE_UNITS, &ship->forward, &ship->up); // turn around to face down
  } else {
    // apply movement calculated on last frame.  (forward vector holds delta x,y,z.)
    ship->location.x += ship->forward.x*ship->speed;
    ship->location.y += ship->forward.y*ship->speed;
    ship->location.z += ship->forward.z*ship->speed;
  }
}

void flightpath2(int Frame, object_t *ship) {
  if (Frame == 0) {
    // Initial and final ship positions for each flight path to be selected semi at random,
    // consistent with the arcade game.
    ship->location.x = 0;
    ship->location.y = 0;
    ship->location.z = 1023;
    ship->speed = 4.0;
    pitch(64*ANGLE_UNITS, &ship->forward, &ship->up); // first, nose down, pilot's head towards us 
    yaw(64*ANGLE_UNITS, &ship->forward, &ship->up); // then turn so that ship is facing left
  } else {
    // apply movement calculated on last frame.  (forward vector holds delta x,y,z.)
    ship->location.x += ship->forward.x*ship->speed;
    ship->location.y += ship->forward.y*ship->speed;
    ship->location.z += ship->forward.z*ship->speed;
  }
}

void flightpath3(int Frame, object_t *ship) {
  if (Frame == 0) {
    // Initial and final ship positions for each flight path to be selected semi at random,
    // consistent with the arcade game.
    ship->location.x = 0;
    ship->location.y = 0;
    ship->location.z = 1023;
    ship->speed = 8.0;
    yaw(128*ANGLE_UNITS, &ship->forward, &ship->up); // turn around to face us
    yaw(16*ANGLE_UNITS, &ship->forward, &ship->up); // slightly off to one side
  } else {
    // apply movement calculated on last frame.  (forward vector holds delta x,y,z.)
    roll(-0.2*ANGLE_UNITS, &ship->forward, &ship->up); // a little bit of motion to show that it's not a single predrawn image...
    ship->location.x += ship->forward.x*ship->speed;
    ship->location.y += ship->forward.y*ship->speed;
    ship->location.z += ship->forward.z*ship->speed;
  }
}

void dump_path(FILE *flightpath, int Frame, int shipno, int waveno) {
  int i;
  
  fprintf(flightpath, "#define SHIP%d_WAVE%d_FRAMES %uU\n", shipno, waveno, Frame);

  fprintf(flightpath, "const uint8_t ship%d_wave1_rx[%uU] = { // rotation vector components\n", shipno, Frame);
  for (i = 0; i < Frame; i++) {
    if ((i & 15) == 0) fprintf(flightpath, "  ");
    fprintf(flightpath, "%3u, ", save_rx[i]);
    if ((i & 15) == 15) fprintf(flightpath, "\n");
  }
  if ((i & 15) != 15) fprintf(flightpath, "\n");
  fprintf(flightpath, "};\nconst uint8_t ship%d_wave%d_ry[%uU] = {\n", shipno, waveno, Frame);
  for (i = 0; i < Frame; i++) {
    if ((i & 15) == 0) fprintf(flightpath, "  ");
    fprintf(flightpath, "%3u, ", save_ry[i]);
    if ((i & 15) == 15) fprintf(flightpath, "\n");
  }
  if ((i & 15) != 15) fprintf(flightpath, "\n");
  fprintf(flightpath, "};\nconst uint8_t ship%d_wave%d_rz[%uU] = {\n", shipno, waveno, Frame);
  for (i = 0; i < Frame; i++) {
    if ((i & 15) == 0) fprintf(flightpath, "  ");
    fprintf(flightpath, "%3u, ", save_rz[i]);
    if ((i & 15) == 15) fprintf(flightpath, "\n");
  }
  if ((i & 15) != 15) fprintf(flightpath, "\n");

  fprintf(flightpath, "};\n\nconst uint8_t ship%d_wave%d_sc[%uU] = { // display scale for ship\n", shipno, waveno, Frame);
  for (i = 0; i < Frame; i++) {
    if ((i & 15) == 0) fprintf(flightpath, "  ");
    fprintf(flightpath, "%3u, ", save_sc[i]);
    if ((i & 15) == 15) fprintf(flightpath, "\n");
  }
  if ((i & 15) != 15) fprintf(flightpath, "\n");

  fprintf(flightpath, "};\nconst int8_t ship%d_wave%d_sx[%uU] = { // screen x,y\n", shipno, waveno, Frame);
  for (i = 0; i < Frame; i++) {
    if ((i & 15) == 0) fprintf(flightpath, "  ");
    fprintf(flightpath, "%4d, ", save_sx[i]);
    if ((i & 15) == 15) fprintf(flightpath, "\n");
  }
  if ((i & 15) != 15) fprintf(flightpath, "\n");
  fprintf(flightpath, "};\nconst int8_t ship%d_wave%d_sy[%uU] = {\n", shipno, waveno, Frame);
  for (i = 0; i < Frame; i++) {
    if ((i & 15) == 0) fprintf(flightpath, "  ");
    fprintf(flightpath, "%4d, ", save_sy[i]);
    if ((i & 15) == 15) fprintf(flightpath, "\n");
  }
  if ((i & 15) != 15) fprintf(flightpath, "\n");

  fprintf(flightpath, "};\n\n");

  fprintf(flightpath, "// This attack path uses %d bytes of EPROM.\n", 6*(Frame+1)+19);
}

int genpath(int shipno, int wave, void flightpath(int Frame, object_t *ship)) {
  // (shipno isn't used yet, since we only do one ship at a time and store them all in ship[0] -
  //  - also, we currently only have the one set of save_* arrays to write the results to...)
  int Frame = 0;  
  Vector right;

  float prx, pry, prz; // TO DO: Move into ship struct.
#ifdef USE_FB
  fbgl_key_t key = 'H';
#endif

  // New ships out of the dockyard face forward (away from us) and always move in the dirction they are facing
  ship[0]->forward = (Vector){0.0, 0.0, 1.0};
  //ship[0]->forward.x =  0.0;
  //ship[0]->forward.y =  0.0;
  //ship[0]->forward.z =  1.0;
  
  ship[0]->up = (Vector){0.0, 1.0, 0.0};
  //ship[0]->up.x = 0.0;
  //ship[0]->up.y = 1.0;
  //ship[0]->up.z = 0.0;

  for (;;) {
    /*
          Things had been going well with flight paths being plausible, when a test
          threw up a weird effect - a ship was moving sideways, which should never
          happen, since the motion is *defined* by the way the ship is facing - so
          my guess was that I had defined the ships using the wrong axes or something
          like that, which I tried to test by mucking about with the project_vertex
          call.  It didn't help!  I *think* I've restored the code I was messing
          with, but there's a danger I missed some part so I need to be really careful
          now while debugging this sideways motion problem that I'm not looking at
          secondary bugs caused by the earlier debugging...

          Apart from the sideways motion, at some point I also have to check that
          the ship's Z axis is set up correctly (with a non-perspective projection,
          the ship looks the same facing away as it would facing towards you) and of
          course x values can be reflected and we'd never notice (though also, we
          just don't care because the ships are symmetrical)  And finally at one
          point I had noticed (or suspected) that the x and y axes had been swapped
          but I don't see that now - it might have been an artifact of hand-built
          orientation vectors rather than setting up the initial orientation once
          and always using the pitch/yaw/roll calls for subsequnt orientation changes.

          Finally it's a little annoying that the Vectrex code uses 256 angles in a
          circle and the linux code uses floating point radians.  I'ld like to update
          the linux interfaces to use 256 angles too (although with floating point
          accuracy, same as code that works in 360 degrees does) but that will have
          to wait until all the flight logic is working correctly.
          
     */
    
    flightpath(Frame, ship[0]);

    save_sx[Frame] = ship[0]->x = project_x(ship[0]->location.x,ship[0]->location.z);
    save_sy[Frame] = ship[0]->y = project_y(ship[0]->location.y,ship[0]->location.z);
    save_sc[Frame] = ship[0]->scale = Scale_fn(ship[0]->location.z); // or project_x(128,z)*2;

    // These tests detect it flying outside the universe
    if (ship[0]->location.x <= -512 || ship[0]->location.x >= 512 ||
        ship[0]->location.y <= -512 || ship[0]->location.y >= 512 ||
        ship[0]->location.z <= -64 || /* longest ship dimension from center */
        ship[0]->scale == 255 ||
        Frame >= 255) {
      fprintf(stderr, "ship[0]->location.x = %f\r\n", ship[0]->location.x);
      fprintf(stderr, "ship[0]->location.y = %f\r\n", ship[0]->location.y);
      fprintf(stderr, "ship[0]->location.z = %f\r\n", ship[0]->location.z);
      fprintf(stderr, "ship[0]->scale = %d\r\n", ship[0]->scale);
      return Frame;
      
    }
    right = cross(ship[0]->up, ship[0]->forward);

    // The pitch/roll/yaw calculations are being done in floating point on a real computer, then will be recorded
    // for use as predetermined flight paths in the Vectrex tailgunner remake, much as was done in the original
    // Cinematronic tailgunner arcade game.  The data to be saved for the Vectrex is all 8 bit -  even the location
    // of the ship in 3D space which is really a 1K cube, because all we're really saving are the x,y screen
    // coordinates of the ship center, and a scale factor for drawing it to represent z distance.  (missile
    // hit detection is really just 2D, also like the original game.)

    get_rotation_parameters(ship[0]->forward, ship[0]->up, &prx, &pry, &prz);

    // Convert to data suitable for storing and reuse as a precomputed fliht path on the Vectrex:
    save_rx[Frame] = iround(prx*128.0/M_PI);
    save_ry[Frame] = iround(pry*128.0/M_PI);
    save_rz[Frame] = iround(prz*128.0/M_PI);

    // There's an implementation problem when the ship origin is to be plotted
    // offscreen ... the trivial problem is that the vectrex coordinates are 8 bits so
    // you can easily place the cursor left of -128 ... but the more significant
    // problem is that we can't *store* coordinates larger than 8 bits or the size
    // of the pre-recorded data balloons.  So we need a game mechanism that allows
    // some special method of handling the ships when they pass the edge of the
    // screen.  And I mean *handling*, not *drawing* - i.e. something that can
    // allows us to not draw them but which makes sense in the game.  For example
    // they might be Kamikaze ships that explode when they get close enough to you.
    // For now, I'm fading them out as they're close to the edge.    
    Output_data(Frame, ship[0], right, prx, pry, prz);

    Wait_Recal();
    Reset0Ref();
    Intensity_7F();

    // TO DO: create a list of pitch/yaw/roll commands to be executed on specific frame
    // numbers and record the display parameters after executing each tick.  Internally
    // keep track of x,y,z position to allow creation of screen x,y location and scale.
    // Handle view angle as well.

    key = fbgl_poll_key(); // poor library interface - short term hack
    if (key == FBGL_KEY_ESCAPE || key == 'Q') {
      fbgl_destroy(&fb);
      exit(EXIT_SUCCESS);
    }

    // TO DO: define scale as a function of z pos in expanded universe

    // As a result of the above, I have a forward-pointing vector which can be used to
    // move the ship's x,y,z position in space, and an upward-pointing (and implicitly
    // a right-pointing) vector that defines its orientation.  The call below converts
    // these into an euler rotation that can be used to render the ship on the screen
    // at a plausible orientation.  Note that this does *not* take into account the
    // X or Y angle from the viewer to the ship (which is projected as if it were at 0,0)
    // but maybe that can be corrected by adding another quaternion representing the
    // head-upward viewing angle when calculating the ship's absolute orientation.

    project_object(ship[0], save_rx[Frame], save_ry[Frame], save_rz[Frame], save_sc[Frame], save_sx[Frame], save_sy[Frame]);

    Frame += 1;
    
  }
}

// #################################### Main entrypoint ########################################


int main(int argc, char **argv) {
  int Frames;

  if (argv[1] != NULL && *argv[1] == '/') {
    (void)InitGraphics(argv[1]);
  } else {
    (void)InitGraphics("/dev/fb1"); // or NULL for default...
  }

  FILE *flightpath = fopen("demo_flightpath.h", "w");
  
  fprintf(flightpath, "// Generated by genpath.c\n");
  fprintf(flightpath, "// An attack wave must be no more than 256 frames, which at 30FPS allows up up to 8 seconds per wave.\n");
  fprintf(flightpath, "// Still to handle: multiple ships and multiple waves, and storing them all in the object_t 'ship' structs.\n");

  // flightpath2(Frame, ship[0]);  // ship moves sideways!!!
                                   // This shows up the bug I'm currently tracking down...

  
  Frames = genpath(/* ship */ 1, /* wave */ 1, flightpath0);
  dump_path(flightpath, Frames, /* ship */ 1, /* wave */ 1);  // simple approach.  Looks OK.

  Frames = genpath(/* ship */ 2, /* wave */ 1, flightpath1);
  dump_path(flightpath, Frames, /* ship */ 2, /* wave */ 1);  // flies from above to below.

  Frames = genpath(/* ship */ 3, /* wave */ 1, flightpath3);
  dump_path(flightpath, Frames, /* ship */ 3, /* wave */ 1);  // simple approach with sideways movement.

  fclose(flightpath);
  
  exit(EXIT_SUCCESS);
  
}