#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <time.h>
#include <math.h>
#include <conio.h>
#include "dpl.h"
#include "matrix.h"
#include "spline.h"

/*{{{  static float atan2deg ( float dz, float dx )*/
static float _atan2deg ( float dz, float dx )
{
  float angle;
  if ((dx*dx) < 0.0001f) {
    angle=(dz>0) ? 90.0f : -90.0f;
    return angle;
  }
  else {
    return (180.0f / 3.1415926535f) * atan2 ( dz, dx );
  }
}
/*}}}  */

/*{{{  static void xformpoint ( POINT q, POINT p, MATRIX m )*/
static void _xformpoint ( dpl_POINT q, dpl_POINT p, dpl_MATRIX m )
{
  dpl_POINT r;
  int i;

  for (i=0; i<4; i++ ) {
    r[i]=(p[0]*m[0][i])+
         (p[1]*m[1][i])+
         (p[2]*m[2][i])+
         (p[3]*m[3][i]);
  }
  memcpy ( q, r, sizeof(dpl_POINT));
}
/*}}}  */

/*{{{  void look_at ( VIEW *v, float *from, float *at, float z_spin )*/
void look_at ( dpl_VIEW *v, float *from, float *at, float z_spin )
{
  /* construct a view from 2 points  */
  float dx, dy, dz;
  float px, py, pz;
  float thetax, thetay, thetaz;
  float yaw, roll;
  dpl_MATRIX m;
  dpl_POINT p, q;

  /* put 'at' at origin, construct a vector from 'from' */
  px=from[0]; py=from[1]; pz=from[2];

  dx=at[0]-px;
  dy=at[1]-py;
  dz=at[2]-pz;

  thetaz=0.0f;
  thetay=0.0f;
  thetax=0.0f;

  p[0]=dx;
  p[1]=dy;
  p[2]=dz;
  p[3]=1;

  yaw=-_atan2deg ( dz, dx );
  dpl_IdMatrix ( m );
  dpl_Rotate   ( m, -yaw, dpl_Y );
  _xformpoint  ( q, p, m );

  roll=_atan2deg(q[1], q[0] );

  /* project onto x-z plane, assuming camera is initially looking down -z */
  dpl_IdMatrix ( v->matrix );

  /* apply angles to matrix */
  
  dpl_Rotate   ( v->matrix, z_spin,    dpl_Z );
  dpl_Rotate   ( v->matrix, roll,      dpl_X );
  dpl_Rotate   ( v->matrix, yaw-90.0f, dpl_Y );

  dpl_Translate( v->matrix, px, py, pz );

}
/*}}}  */

static int camera_inited=0;
static float camera_t=0.0f;
static cntl_point *camerahead=NULL;

/*{{{  void camera_move ( VIEW *eye,*/
void camera_move ( dpl_VIEW *eye,
                     char *spline_file,
                     dpl_POINT look_at_point,
                     float scale_fac,
                     float dt )
{
  cntl_point *p=camerahead;
  float t=camera_t;
  float px, py, pz, rx, ry, rz;
  dpl_POINT evalp;

  if (camera_inited == 0) {
    camera_inited=1;
    p=init_splines(spline_file);
    t=0;
  }
  
  px=eval(p->x_pos_cubic, t );
  py=eval(p->y_pos_cubic, t );
  pz=eval(p->z_pos_cubic, t );

  rx=eval(p->x_rot_cubic, t );
  ry=eval(p->y_rot_cubic, t );
  rz=eval(p->z_rot_cubic, t );

  camerahead=walk_spline ( p, &t, dt );  
  camera_t=t;

  /* we have wobbled the spline according to the file - now build the
     view matrix */
  /*
  eye->matrix[3][0]=px+look_at_point[0];
  eye->matrix[3][1]=py+look_at_point[1];
  eye->matrix[3][2]=pz+look_at_point[2];
 
  look_at ( eye, &eye->matrix[3][0], look_at_point, rz );
  */

  dpl_IdMatrix  ( eye->matrix );

  dpl_Rotate    ( eye->matrix, rz,   dpl_Z );
  dpl_Rotate    ( eye->matrix, rx,   dpl_X );
  dpl_Rotate    ( eye->matrix, ry,   dpl_Y );
  dpl_Translate ( eye->matrix, px, py, pz );

  dpl_Flush(eye);
}
/*}}}  */
/*{{{  void camera_move_kbd ( VIEW *eye,*/

static float 
    yaw = 0.0f, pitch = 0.0f, roll = 0.0f,
    x_velocity = 0.0f,
    y_velocity = 0.0f,
    z_velocity = 0.0f,
    px = 0.0f, py = 5.0f, pz = 0.0f,
    pitch_velocity = 0.0f,
    yaw_velocity   = 0.0f,
    roll_velocity  = 0.0f,
    dspin = 0.3f, dspeed = 0.3f;

static int   
    enhanced_key = 0;  //Flag:  True if next char is enhanced key.

void camera_move_kbd ( dpl_VIEW *eye,
                       char *spline_file,
                       dpl_POINT look_at_point,
                       float scale_fac,
                       float dt )
{
    dpl_POINT vel0, vel1;

    /* compute roll, pitch, yaw from keyboard input */

    if (kbhit()) {
        int ch;

        ch = getch();
        if (enhanced_key) {
            enhanced_key = 0; //False
            switch (ch) {
             case 'H': // 0,72 'H' up arrow
                y_velocity+=dspeed;
                break;
             case 'P': // 0,80 'P' down arrow
                y_velocity-=dspeed;
                break;
             case 'M': // 0,77 'M' left arrow
                x_velocity+=dspeed;
                break;
             case 'K': // 0,75 'K' right arrow
                x_velocity-=dspeed;
                break;
            }
        }
        else if (ch == 0) {
            enhanced_key = -1; //True
        }
        else {
            switch (ch) {
             case '7' :  /* roll left */
                roll_velocity += dspin;
                break;
             case '9':  /* roll right */
                roll_velocity -= dspin;
                break;
             case '4' :  /* swing left weeet chariot */
                yaw_velocity+=dspin;
                break;
             case '6' :  /* right */
                yaw_velocity-=dspin;
                break;
             case '8' :  /* upp */
                pitch_velocity-=dspin;
                break;
             case '2' : /* downnn */
                pitch_velocity+=dspin;
                break;
             case '5' : /* go faster */
                z_velocity+=(dspeed+dspeed);
                break;
             case '0' : /* decelerate */
                z_velocity-=(dspeed+dspeed);
                break;
             case '-':
                pitch = 0.0;
                yaw = 0.0;
                roll = 0.0;
                break;
             case ' ' : /* set speed to 0 */
                x_velocity=0.0f;
                y_velocity=0.0f;
                z_velocity=0.0f;
                // v--fall through--v //
             case '*' :
                pitch_velocity = 0.0f;
                yaw_velocity = 0.0f;
                roll_velocity = 0.0f;
                break;
            }
        }
    }

    vel0[0]=x_velocity;
    vel0[1]=y_velocity;
    vel0[2]=-z_velocity;

    dpl_IdMatrix  ( eye->matrix );

    roll  += roll_velocity;
    pitch += pitch_velocity;
    yaw   += yaw_velocity;

    dpl_Rotate    ( eye->matrix, roll,  dpl_Z );
    dpl_Rotate    ( eye->matrix, pitch, dpl_X );
    dpl_Rotate    ( eye->matrix, yaw,   dpl_Y );

    _xformpoint ( vel1, vel0, eye->matrix );

    px+=vel1[0];
    py+=vel1[1];
    pz+=vel1[2];

    dpl_Translate ( eye->matrix, px, py, pz );

    dpl_Flush ( eye );
}
/*}}}  */