// Compile with -O2 option
#include <vectrex.h>
#define set_scale(A) dp_VIA_t1_cnt_lo = A

// define this to test with a single object (which does run under 30K cycles)
// #define JUST_ONE 1

#define int8_t int
#define uint8_t unsigned int
#define int16_t long

// Note: doesn't correct for slightly non-square aspect ratio,
//       so a cube will be stretched vertically a smidgeon.
//       Also, we don't handle perspective, but who cares?

#define MAX_VERTICES 100

typedef struct {
    int8_t x, y;
} screen;

screen projected_vertices[MAX_VERTICES]; // As many vertices as largest object requires.

static inline int8_t fixed_mul(int8_t a, int8_t b) {
  return (int8_t)(((int16_t)a * b) >> 6L);
}

// Although this table could be shortened, a full table is faster...
static const int8_t sin_table[256] = {
  0, 2, 3, 5, 6, 8, 9, 11, 12, 14, 16, 17, 19, 20, 22, 23,
  24, 26, 27, 29, 30, 32, 33, 34, 36, 37, 38, 39, 41, 42, 43, 44,
  45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 56, 57, 58, 59,
  59, 60, 60, 61, 61, 62, 62, 62, 63, 63, 63, 64, 64, 64, 64, 64,
  64, 64, 64, 64, 64, 64, 63, 63, 63, 62, 62, 62, 61, 61, 60, 60,
  59, 59, 58, 57, 56, 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46,
  45, 44, 43, 42, 41, 39, 38, 37, 36, 34, 33, 32, 30, 29, 27, 26,
  24, 23, 22, 20, 19, 17, 16, 14, 12, 11, 9, 8, 6, 5, 3, 2,
  0, -2, -3, -5, -6, -8, -9, -11, -12, -14, -16, -17, -19, -20, -22, -23,
  -24, -26, -27, -29, -30, -32, -33, -34, -36, -37, -38, -39, -41, -42, -43, -44,
  -45, -46, -47, -48, -49, -50, -51, -52, -53, -54, -55, -56, -56, -57, -58, -59,
  -59, -60, -60, -61, -61, -62, -62, -62, -63, -63, -63, -64, -64, -64, -64, -64,
  -64, -64, -64, -64, -64, -64, -63, -63, -63, -62, -62, -62, -61, -61, -60, -60,
  -59, -59, -58, -57, -56, -56, -55, -54, -53, -52, -51, -50, -49, -48, -47, -46,
  -45, -44, -43, -42, -41, -39, -38, -37, -36, -34, -33, -32, -30, -29, -27, -26,
  -24, -23, -22, -20, -19, -17, -16, -14, -12, -11, -9, -8, -6, -5, -3, -2,
};

static inline int8_t sin_fixed(uint8_t angle) {
    return sin_table[angle];
}

static inline int8_t cos_fixed(uint8_t angle) {
    return sin_table[angle + 64U]; // &0xFF is implicit using 8-bit ints
}

// Objects are stored as 8-bit coordinates centered on 0,0,0

// A model is a list of vertices, plus a list of lines to be drawn from vertex to vertex.

// Sin and Cos tables are lookups returning 8 bit values in the range -64:64, where
// 64 represents 1.0f and -64 is -1.0f  This is to avoid the need for an extra byte
// in calculations if -128:128 were used, or expensive divides if -127:127 were used.

// We will apply the transformations to the vertices first, then draw the ship using
// the transformed vertices.  This differs from the original code where each line was
// being transformed resulting in redundant transformations being calculated any time
// two lines met at a point.  Drawing is optimised by only issuing a move when the
// next line does not follow the proceding line.  In those cases a reset0ref is also
// performed.

typedef struct {
    int8_t x, y, z;
} point3d;

typedef struct {
    int8_t from, to;
} edge_t;

typedef struct {
  const edge_t *edges;
  const point3d *vertices;
  const uint8_t count;
} object_t;

// A test model:
#define CUBE_VERTICES 8
#define CUBE_SIZE 62 // No model coordinates larger than this value.
point3d cube_vertices[CUBE_VERTICES] = {
    {-CUBE_SIZE, -CUBE_SIZE, -CUBE_SIZE},
    { CUBE_SIZE, -CUBE_SIZE, -CUBE_SIZE},
    { CUBE_SIZE,  CUBE_SIZE, -CUBE_SIZE},
    {-CUBE_SIZE,  CUBE_SIZE, -CUBE_SIZE},
    {-CUBE_SIZE, -CUBE_SIZE,  CUBE_SIZE},
    { CUBE_SIZE, -CUBE_SIZE,  CUBE_SIZE},
    { CUBE_SIZE,  CUBE_SIZE,  CUBE_SIZE},
    {-CUBE_SIZE,  CUBE_SIZE,  CUBE_SIZE}
};
const edge_t cube_edges[] = { // values are from,to indexes into a vertex array
    {0, 1}, {1, 2}, {2, 3}, {3, 0},
    {4, 5}, {5, 6}, {6, 7}, {7, 4},
    {0, 4}, {1, 5}, {2, 6}, {3, 7},
    {-1,-1}
};
object_t cube0 = {
  cube_edges, cube_vertices, CUBE_VERTICES
};
object_t cube1 = {
  cube_edges, cube_vertices, CUBE_VERTICES
};
object_t cube2 = {
  cube_edges, cube_vertices, CUBE_VERTICES
};

static inline void draw_object(const edge_t *edge, screen *vertex, uint8_t scale, int8_t ox, int8_t oy) {
  int8_t i = 0, from, to, last_to = -1, vfx, vfy;
  set_scale(scale);
  for (;;) {
    if ((from = edge[i].from) < 0) return;
    to = edge[i].to; vfx = vertex[from].x; vfy = vertex[from].y;
    if (from != last_to) { Reset0Ref(); Moveto_d(oy, ox);  Moveto_d(vfy, vfx); }
    last_to = to;
    Draw_Line_d(vertex[to].y - vfy, vertex[to].x - vfx);
    i += 1;
  }
}

static inline void project_vertex(const point3d *v, uint8_t ax, uint8_t ay, uint8_t az, screen *vec) {
    int8_t x = v->x;
    int8_t y = v->y;
    int8_t z = v->z;

    // Rotate around X axis
    int8_t cosy = cos_fixed(ax);
    int8_t siny = sin_fixed(ax);
    int8_t y1 = fixed_mul(y, cosy) - fixed_mul(z, siny);
    int8_t z1 = fixed_mul(y, siny) + fixed_mul(z, cosy);
    y = y1; z = z1;

    // Rotate around Y axis
    int8_t cosx = cos_fixed(ay);
    int8_t sinx = sin_fixed(ay);
    int8_t x1 =  fixed_mul(x, cosx) + fixed_mul(z, sinx);
    int8_t z2 = -fixed_mul(x, sinx) + fixed_mul(z, cosx);
    x = x1; z = z2;

    // Rotate around Z axis
    int8_t cosz = cos_fixed(az);
    int8_t sinz = sin_fixed(az);
    int8_t x2 = fixed_mul(x, cosz) - fixed_mul(y, sinz);
    int8_t y2 = fixed_mul(x, sinz) + fixed_mul(y, cosz);

    // A simple x,y projection will suffice!
    vec->x = x2;
    vec->y = y2;
}

static inline void project_object(const object_t *object, uint8_t angle_x, uint8_t angle_y, uint8_t angle_z, screen *output_vectors) {
  for (uint8_t i = 0; i < object->count; i++) {
    project_vertex(&object->vertices[i], angle_x, angle_y, angle_z, &output_vectors[i]);
  }
}

int main(void) {
    uint8_t angle_x[3] = {0,0,0}, angle_y[3] = {0,0,0}, angle_z[3] = {0,0,0};  // 0:255 angles make a circle.

    uint8_t scale = 5;  // We'll use hardware scaling and adjust it separately from the viewing angle transformation
    int8_t scale_dir = 1; // forward or backward
    int8_t delay = 3;

    for (;;) {
      Wait_Recal();
      Reset0Ref();
      Intensity_7F();
      // Scale up/down to simulate approach and passing
      if (delay == 0) {      
        scale = (uint8_t)((int8_t)scale + scale_dir);
        if (scale > 140 || scale < 5) scale_dir = -scale_dir; // forward+reverse for testing
        delay = 3;
      }
      delay -= 1;
      angle_x[0] += 2; angle_y[0] += 3; angle_z[0] += 1;  // Rotate the objects
#ifndef JUST_ONE
      angle_x[1] += 1; angle_y[1] += 2; angle_z[1] += 3;
      angle_x[2] += 3; angle_y[2] += 1; angle_z[2] += 2;
#endif
      project_object(&cube0, angle_x[0], angle_y[0], angle_z[0], projected_vertices);
      draw_object(cube_edges, projected_vertices, scale,    0,  60);
#ifndef JUST_ONE
      project_object(&cube1, angle_x[1], angle_y[1], angle_z[1], projected_vertices);
      draw_object(cube_edges, projected_vertices, scale+20, -120, -60);
      project_object(&cube2, angle_x[2], angle_y[2], angle_z[2], projected_vertices);
      draw_object(cube_edges, projected_vertices, scale+40,  120, -60);
#endif
    }
}