/*
   This code transforms any 3D point in space to the corresponding 2D position as viewed
   on the screen.  It's still draft code and I'll wait until it is all working robustly
   before writing it up.  There are currently two different 'work in progress' sets of
   3D-handling code here as I'm in the process of converting a crude initial hack into
   something that uses transformation matrices to perform multiple transformations
   simultaneously.  This code will also eventually handle the camera viewpoint, allowing
   for a moving camera (eg as in Battlezone where it rotates through the 360 degree
   field of view.
 */

static void do_3d_abs(const coord *coord, void (*generic_abs)(int, int)) {
  int x2d, y2d;
  float newx, newy;
#define X (coord[0]-center[0])
#define Y (coord[1]-center[1])
#define Z (coord[2]-center[2])
  // need to add the code from drawships.c ...
  newx = X * cosf(angle_rad) - Y * sinf(angle_rad);  // just as a test, rotating around the z axis
  newy = X * sinf(angle_rad) + Y * cosf(angle_rad);
  x2d = (int32_t)(focal_length * newx / Z);
  y2d = (int32_t)(focal_length * newy / Z);
  generic_abs(y2d, x2d);
#undef X
#undef Y
#undef Z
}

static inline void moveto_3d_abs(const coord *coord) {
  do_3d_abs(coord, moveto_abs);
}

static inline void lineto_3d_abs(const coord *coord) {
  do_3d_abs(coord, lineto_abs);
}

static inline void line_3d(Model *m, const vertexidx v1, const vertexidx v2) {
  moveto_3d_abs(m->vertex[v1].coord);
  lineto_3d_abs(m->vertex[v2].coord);
}

static inline void edge_3d(Model *m, const edgeidx e) {
  line_3d(m, m->edge[e].from, m->edge[e].to);
}

#define X 0
#define Y 1
#define Z 2
void matmult(const Matrix4x4 *m, const Vertex *v, Vertex *result) {
  result->coord[X] = m->m[0][0] * v->coord[X] + m->m[0][1] * v->coord[Y] + m->m[0][2] * v->coord[Z] + m->m[0][3];
  result->coord[Y] = m->m[1][0] * v->coord[X] + m->m[1][1] * v->coord[Y] + m->m[1][2] * v->coord[Z] + m->m[1][3];
  result->coord[Z] = m->m[2][0] * v->coord[X] + m->m[2][1] * v->coord[Y] + m->m[2][2] * v->coord[Z] + m->m[2][3];
}

void project3d(const Vertex *vertex, const Matrix4x4 *model_orientation, const Vertex *absolute_camera_position, const float focal_length,
               const int screen_width, const int screen_height, int *px, int *py) {
  Vertex world, relative_camera_position;
  
  matmult(model_orientation, vertex, &world); // Apply model orientation
  
  // Convert to camera-relative coordinates
  relative_camera_position.coord[X] = world.coord[X] - absolute_camera_position->coord[X];
  relative_camera_position.coord[Y] = world.coord[Y] - absolute_camera_position->coord[Y];
  relative_camera_position.coord[Z] = world.coord[Z] - absolute_camera_position->coord[Z];
  
  if (relative_camera_position.coord[Z] <= 0) { // Behind camera
    *px = -1; *py = -1; // Need to 3d-clip against viewport
    return;
  }
  
  // Perspective projection
  float x_proj = (focal_length * relative_camera_position.coord[X]) / relative_camera_position.coord[Z];
  float y_proj = (focal_length * relative_camera_position.coord[Y]) / relative_camera_position.coord[Z];
  
  // Map to viewport coordinates.  Need to clip to 2d viewport.
  *px = (int)((x_proj * screen_width) / (2 * focal_length) + screen_width / 2);
  *py = (int)(screen_height / 2 - (y_proj * screen_height) / (2 * focal_length));
}
#undef X
#undef Y
#undef Z

void draw_object(Model *m,
                 const Matrix4x4 *model_orientation, const Vertex *camera,
                 const float focal_length, const int screen_width, const int screen_height) {
  for (edgeidx e = 0; e < m->max_edges; e++) {
    Vertex a, b;
    int ax, ay, bx, by;
    a = m->vertex[m->edge[e].from]; b = m->vertex[m->edge[e].to];
    project3d(&a, model_orientation, camera, focal_length, screen_width, screen_height, &ax, &ay);
    project3d(&b, model_orientation, camera, focal_length, screen_width, screen_height, &bx, &by);
    if (ax >= 0 && ay >= 0 && bx >= 0 && by >= 0) {
      fprintf(stderr, "edge%d: Line(%d, %d,  /*to*/  %d, %d)\n", e, ax, ay, bx, by);
    }
  }
}

// #define USE_FACES 1  // Test the extracted face data to compare against drawing only the edges.
static void draw_model_3d(Model *m) {
#ifdef USE_FACES
  fprintf(stderr, "%d faces\n", next_free_face);
  for (faceidx f = 0; f < next_free_face; f++) {
    // For the demo, we want to set the colour of the next face to be highlighted, and leave it for a few seconds.
    for (edgeidx e = 0; e < face[f].num_nodes_in_face; e++) {
      // each vertex on the cyclic path around any face should have a corresponding edge leading to the next face.
      // (I hope that the final vertex correctly wraps around to the start!)
      // In an 8-bit game with backface culling, 'drawing' a face would map to setting bits representing each edge
      // of that face, then looping through the unique edge array, drawing only those edges which were marked.  This
      // avoids the problem of drawing edges that are shared by two faces twice.
      edge_3d(m, face[f].edge[e]);
    }
  }
#else // !USE_FACES

#ifdef USE_CAMERA
#define SCREEN_WIDTH 256
#define SCREEN_HEIGHT 256
  Matrix4x4 orientation = {
    .m = {
      {1.0f, 0.0f, 0.0f, 0.0f},
      {0.0f, 1.0f, 0.0f, 0.0f},
      {0.0f, 0.0f, 1.0f, 0.0f},
      {0.0f, 0.0f, 0.0f, 1.0f}
    }
  };
  Vertex camera = {{128,128,0}};
  draw_object(m, &orientation, &camera, focal_length, SCREEN_WIDTH, SCREEN_HEIGHT);
#else // !USE_CAMERA
  for (edgeidx e = 0; e < m->max_edges; e++) {
    edge_3d(m, e);
  }
#endif // !USE_CAMERA
#endif // !USE_FACES
}