/*
    SABRE Fighter Plane Simulator 
    Copyright (c) 1997 Dan Hammer
    Portions Donated By Antti Barck

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 1, or (at your option)
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
/*************************************************
 *           SABRE Fighter Plane Simulator              *
 * Version: 0.1                                  *
 * File   : port_3d.C                            *
 * Date   : March, 1997                          *
 * Author : Dan Hammer                           *
 *************************************************/
#include <stdio.h>
#include <math.h>
#include <iostream.h>
#include <fstream.h>
#include "vmath.h"
#include "rtkey.h"
#include "vga_13.h"
#include "port_3d.h"

const float pi = 3.1415927;
const float half_pi = 1.5707963;
const float two_pi = 6.2831853;
const float three_fourth_pi = 4.712389;
const float _degree = 0.0174533;
const float _minute = 2.908882e-04;
const float _second = 4.848137e-06;

Rect Port_3D::screen(0,0,SCREEN_WIDTH-1,SCREEN_HEIGHT-1);
REAL_TYPE Port_3D::cx = SCREEN_WIDTH / 2;
REAL_TYPE Port_3D::cy = SCREEN_HEIGHT / 2;
REAL_TYPE Port_3D::fovx = 300;
REAL_TYPE Port_3D::fovy = 280;
extern REAL_TYPE world_scale;

void Port_3D::read(istream &is)
{
  /* used to be fovx */
  float ignore;
 
  is >> slook_from >> look_from;
  look_from *= world_scale;
  is >> ignore >> roll;
  is  >> z_min >> z_max >> horizon;
  calc_angles();
  calc_look_at();
  calc_view_normal();
  sin_roll = sin(roll);
  cos_roll = cos(roll);
}

istream &operator >>(istream &is, Port_3D &port)
{
  port.read(is);
  return is;
}

void Port_3D::write(ostream &os)
{
  os << slook_from  << "\n";
  os << look_from << "\n";
  os << look_at << "\n";
}

ostream &operator <<(ostream &os, Port_3D &port)
{
  port.write(os);
  return os;
}

void Port_3D::initPort3D()
{
  screen = Rect(0,0,SCREEN_WIDTH-1,SCREEN_HEIGHT-1);
  cx = screen.topLeft.x + ((screen.botRight.x - screen.topLeft.x) / 2);
  cy = screen.topLeft.y + ((screen.botRight.y - screen.topLeft.y) / 2);
}

void Port_3D::setScreen(Rect &scr)
{
  screen = scr;
  cx = screen.topLeft.x + ((screen.botRight.x - screen.topLeft.x) / 2);
  cy = screen.topLeft.y + ((screen.botRight.y - screen.topLeft.y) / 2);
}

Port_3D::Port_3D()
  :look_from(0,0,0),
   slook_from(0,0,20),
   roll(0)
{
  calc_rho();
  calc_angles();
  calc_look_at();
  calc_view_normal();
  sin_roll = sin(roll);
  cos_roll = cos(roll);
  z_min = 1.0;
  z_max = 160.0;
  r = 0.0;
  horizon = 100;
}


void Port_3D::copy(Port_3D &p)
{
  look_from = p.look_from;
  look_at = p.look_at;
  slook_from = p.slook_from;
  view_normal = p.view_normal;
  z_min = p.z_min;
  z_max = p.z_max;
  horizon = p.horizon;
  cos_theta = p.cos_theta;
  sin_theta = p.sin_theta;
  cos_phi = p.cos_phi;
  sin_phi = p.sin_phi;
  r = p.r;
  roll = p.roll;
  sin_roll = p.sin_roll;
  cos_roll = p.cos_roll;
  over_flow = p.over_flow;
}

Port_3D::Port_3D(Port_3D &p)
{
  copy(p);
}

Port_3D &Port_3D::operator =(Port_3D &p)
{
  copy(p);
  return (*this);
}

Port_3D::Port_3D(S_3DPoint &sfrom, R_3DPoint &at, REAL_TYPE , REAL_TYPE p)
{
  roll = p;
  slook_from = sfrom;
  look_at = at;
  z_min = 1.0;
  z_max = 160.0;
  calc_angles();
  calc_look_at();
  calc_view_normal();
  sin_roll = sin(roll);
  cos_roll = cos(roll);
  horizon = 100;
}

// Project a point (in port coordinants, i.e. xE,yE,zE) onto the screen
// Note that if we are using FixedPoint, a port.z value < 1.0 will
// very likely result in a divide overflow.
void Port_3D::port2screen(R_3DPoint &port,
			  int *screen_x, int *screen_y)
{
  REAL_TYPE f_d1,f_d2;
  over_flow = 0;
  if (port.z <= 0)
    {
      over_flow = 1;
      return;
    }
  R_KEY_BEGIN(2)
    f_d1 = (fovx * port.x) / port.z;
  f_d2 = (fovy * port.y * aspect_ratio) / port.z;
  if (fabs(f_d2) > 32000.0 ||
      fabs(f_d1) > 32000.0 )
    {
      over_flow = 1;
      R_KEY_END
	return;
    }
  else
    {
      *screen_x = (int) (f_d1 + cx);
      *screen_y = (int) ((f_d2 * -1.0) + cy);
    }
  R_KEY_END
    }

void Port_3D::screen2port(float screen_x, float screen_y, R_3DPoint *port, int flg)
{
  if (flg)
    {
      screen_x -= cx;
      screen_y = -screen_y - cy;
    }
  port->x = port->z * (screen_x / fovx);
  port->y = port->z * (screen_y / (fovy * aspect_ratio));
}

// Calculate 3 points in world coordinants representing the left, center
// and right expanse of the horizon
REAL_TYPE vh_limit = 0.009;
void Port_3D::get_view_horizon(REAL_TYPE dst, R_3DPoint *wh_left,
			       R_3DPoint *wh_center,
			       R_3DPoint *wh_right,
			       R_3DPoint *ref)
{
  REAL_TYPE sv_roll;
  REAL_TYPE mag;
  R_3DPoint p,d;

  // Get the view normal on the xy plane
  d = look_at - look_from;
  mag = sqrt((d.x*d.x)+(d.y*d.y));
  if (mag < vh_limit)
    mag = vh_limit;
  d.x = d.x / mag;
  d.y = d.y / mag;

  // Place the horizon @ requested distance
  if (ref)
    {
      p.x = (ref->x + (d.x * dst));
      p.y = (ref->y + (d.y * dst));
    }
  else
    {
      p.x = (look_from.x + (d.x * dst));
      p.y = (look_from.y + (d.y * dst));
    }
  p.z = 0;

  *wh_center = p;
  // Now comes the serious stuff!
  // We need to get two world points which,
  // when transformed, will be on the edge of
  // the screen
  // Create a new view with a 0 roll
  sv_roll = roll;
  set_roll(0);
  R_3DPoint p1;
  // Get a local coords of the calculated point on the horizon
  world2port(p,&p1);
  // Set some limit on how small z can be
  if (fabs(p1.z) < 0.0001)
    p1.z = 0;

  // 'unproject' an screen-x value of -30 & 349 to get the
  // world coords of points on the horizon which will
  // meet/overlap the edge of the screen
  REAL_TYPE c = (REAL_TYPE) cx;
  p1.x = ((-(p1.z * (SCREEN_WIDTH + SCREEN_HEIGHT))) - (c * p1.z)) / fovx;
  port2world(p1,wh_left);
  p1.x = (((SCREEN_WIDTH + SCREEN_HEIGHT) *  p1.z) - (c * p1.z)) / fovx;
  port2world(p1,wh_right);
  set_roll(sv_roll);
}

void Port_3D::set_view(const R_3DPoint &lf, const R_3DPoint &la)
{
  float dx,dy,dz;
  float dtheta,dphi,dr;
  int which_quad;

  dx = lf.x - la.x;
  dy = lf.y - la.y;
  dz = lf.z - la.z;
  if (dx >= 0.0 && dy >= 0.0)
    which_quad = 1;
  else if (dx > 0.0 && dy < 0.0)
    which_quad = 2;
  else if (dx < 0.0 && dy < 0.0)
    which_quad = 3;
  else
    which_quad = 4;
  dr = sqrt(dx*dx + dy*dy);
  slook_from.rho = 1.0;
  if (dr == 0.0)
    dtheta = slook_from.theta;
  else
    {
      dtheta = acos(dx/dr);
      if (dy <= 0.0)
	dtheta = two_pi - dtheta;
    }
  if (dz == 0.0)
    dphi = half_pi;
  else if (dr == 0.0)
    dphi = 0.0;
  else
    {
      dphi = atan(dr/dz);
      if (dphi < 0.0)
	{
	  if (which_quad < 3)
	    dphi = pi + dphi;
	  else
	    {
	      dphi = pi -  dphi;
	    }
	}
      else if (which_quad == 3)
	dphi = two_pi - dphi;
    }

  if (dphi > pi)
    {
      dphi = two_pi - dphi;
    }
  slook_from.phi = dphi;
  slook_from.theta = dtheta;
  look_from = lf;
  calc_angles();
  calc_look_at();
}

/*********************************************************
 * The following function came into being as a result    *
 * of a problem encountered when applying pitch and yaw  *
 * to the flight. (See flt_cmpl.C,apply_rotations.). The *
 * roll needs to be re-calculated. The method, which is  *
 * embarassingly stupid, is to get the angle ofroll relative to *
 * the line between the current origin and the one we're *
 * rotating to (the 'la' argument in the function below),*
 * move the port via set_view, than re-calculate the roll*
 * so that angle remains constant. The only thing I can  *
 * say about it is it works, but undoubtedly I'm missing *
 * something.
 *********************************************************/
void Port_3D::align_port(R_3DPoint &la)
{
  R_3DPoint p1,p2;
  int dir;
  float d_roll;
  R_3DPoint testpoint,testpoint2;

  float r_dot1,r_measure1,r_dot2,r_measure2;
  Vector d_v,d_r,d_r1;
  Vector victor;

  if (la == look_at)
    return;

  // Calculare roll relative to the line of motion
  victor = R_3DPoint(la - look_at);
  victor.Normalize();
  port2world(R_3DPoint(1,0,0),&p1);
  p1 -= look_from;
  d_v = victor;
  d_r = p1;
  d_r.Normalize();
  d_v.Normalize();
  r_dot1 = d_v.X * d_r.X + d_v.Y * d_r.Y + d_v.Z * d_r.Z;
  //	r_dot1 = d_v.Dot(d_r);
  if (r_dot1 >= -1.0 && r_dot1 <= 1.0)
    r_measure1 = acos(r_dot1);
  else
    {
      //		printf("bad dot1: %f\n",r_dot1);
      return;
    }
  testpoint = look_from;
  testpoint2 = testpoint + R_3DPoint(victor);
  world2port(testpoint2,&p2);
  if (p2.y < 0)
    dir = 1;
  else
    dir = 0;

  set_view(look_from,la);

  port2world(R_3DPoint(1,0,0),&p1);
  p1 -= look_from;
  d_r1 = p1;
  d_r1.Normalize();

  float  test = d_r1.Dot(d_r);
  if (test < 0)
    {
      roll = limit_angle(roll + _PI);
      sin_roll = sin(roll);
      cos_roll = cos(roll);
      port2world(R_3DPoint(1,0,0),&p1);
      p1 -= look_from;
      d_r1 = p1;
      d_r1.Normalize();
    }

  r_dot2 = d_v.Dot(d_r1);
  if (r_dot2 != r_dot1)
    {
      if (r_dot2 >= -1.0 && r_dot2 <= 1.0)
	{
	  r_measure2 = acos(r_dot2);
	  float diff = r_measure2 - r_measure1;
	  if (dir)
	    diff = -diff;
	  if (fabs(diff) > pi)
	    diff = -diff;
	  d_roll = roll;
	  d_roll += diff;
	  if (d_roll < 0.0)
	    d_roll = two_pi + d_roll;
	  if (d_roll >= two_pi)
	    d_roll = d_roll - two_pi;
	  roll = d_roll;
	  sin_roll = sin(roll);
	  cos_roll = cos(roll);
	}
      else
	{
	  //			printf("Bad dot2: %f\n"), r_dot2;
	  return;
	}
    }
}

/* 
   Calculate the roll which would be needed to align
   given point, given in world coordinants, with the 
   given port
   */
float calcRollForPoint(Port_3D &port, R_3DPoint &w)
{
  float result = 0.0;
  Port_3D vport;
  R_3DPoint p;
  float sc_x,sc_y;
  vport = port;
  vport.set_roll(0.0);
  if (world2portN(w,&p,&sc_x,&sc_y,vport))
    {
      result = -atan(p.x / (p.y + eps));
      if (p.y < 0.0)
	result -= _PI;
      if (result < 0.0)
	result = _2PI + result;
    }
  return (result);
}

float calcRollForPoint2(Port_3D &port, R_3DPoint &w, R_3DPoint *p, float *sc_x, float *sc_y)
{
  float result = 0.0;
  Port_3D vport;
  vport = port;
  vport.set_roll(0.0);
  if (world2portN(w,p,sc_x,sc_y,vport))
    {
      if (p->y < 0.0)
	{
	  p->y *= -1.0;
	  //	  p.x *= -1.0;
	}
      result = -atan(p->x / (p->y + eps));
      if (p->y < 0.0)
	result -= _PI;
      if (result < 0.0)
	result = _2PI + result;
    }
  return (result);
}