#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <libvu0.h>
#include <memory.h>
#include <linux/ps2/gs.h>
#include "ps2gs.h"
#include "ps2dma.h"
#include "ps2vpu.h"
#include "ps2vpufile.h"
#include "libvu0.h"
#include "gas.h"
#include "blow.h"
#include "physics.h"
#include "sjoy.h"
#include "ps2base.h"

//----------------------------------------------------------------------
IMPORT_VPU_SYMBOL(u_int *, My_part_src, 0)
IMPORT_VPU_SYMBOL(float *, My_ground_matrix, 0)
IMPORT_VPU_SYMBOL(__u128 *, My_texture1, 0)
IMPORT_VPU_SYMBOL(__u128 *, My_texture3, 0)
IMPORT_VPU_SYMBOL(__u128 *, My_dma_start, 0)

IMPORT_VPU_SYMBOL(__u128 *, My_dma_start_part_fire, 0)
IMPORT_VPU_SYMBOL(__u128 *, My_dma_start_shadow_part_fire, 0)
IMPORT_VPU_SYMBOL(float *, My_matrix_fire, 0)
IMPORT_VPU_SYMBOL(float *, My_shadow_matrix_fire, 0)
IMPORT_VPU_SYMBOL(float *, My_offset_fire, 0)
IMPORT_VPU_SYMBOL(float *, My_shadow_offset_fire, 0)
IMPORT_VPU_SYMBOL(__u128 *, My_texture2_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part1_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part_second_pole_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part_third_pole_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part1_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part_pole2_fire, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part_pole3_fire, 0)

IMPORT_VPU_SYMBOL(__u128 *, My_dma_start_part_gas, 0)
IMPORT_VPU_SYMBOL(__u128 *, My_dma_start_shadow_part_gas, 0)
IMPORT_VPU_SYMBOL(float *, My_matrix_gas, 0)
IMPORT_VPU_SYMBOL(float *, My_shadow_matrix_gas, 0)
IMPORT_VPU_SYMBOL(float *, My_offset_gas, 0)
IMPORT_VPU_SYMBOL(float *, My_shadow_offset_gas, 0)
IMPORT_VPU_SYMBOL(__u128 *, My_texture2_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part1_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part_second_pole_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_part_third_pole_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part1_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part_pole2_gas, 0)
IMPORT_VPU_SYMBOL(u_int *, My_shadow_part_pole3_gas, 0)

int g_fire;
int g_image_size;
float g_part_size;
float g_part_size_shadow;

float *g_my_offset;
float *g_my_shadow_offset;
float *g_my_matrix;
float *g_my_shadow_matrix;
__u128 *g_my_texture2;

__u128 *g_my_dma_start_part;
__u128 *g_my_dma_start_shadow_part;

u_int fire_bound[FIRE_NUM_BLOCK * FIRE_NUM_PART * FIRE_NUM_EXPLODE];

TexEnv texenv1, texenv2;

struct ps2_image g_img1, g_img2, g_img3;

void load_teximages( int texad64 )
{
  int size;
	
  // image 1
  ps2_gs_set_image(&g_img1, texad64, g_image_size / 64, PS2_GS_PSMCT32, 0, 0, g_image_size, g_image_size, My_texture1);
  size = ps2_gs_load_image(&g_img1);
  texad64 += size / 64;
	
  // image 2
  ps2_gs_set_image(&g_img2, texad64, 1, PS2_GS_PSMCT32, 0, 0, 32, 32, g_my_texture2);
  size = ps2_gs_load_image(&g_img2);
  texad64 += size / 64;
	
  // image 3
  ps2_gs_set_image(&g_img3, texad64, 1, PS2_GS_PSMCT32, 0, 0, 32, 32, My_texture3);
  size = ps2_gs_load_image(&g_img3);
  texad64 += size / 64;
}

class DemoPs2 : public Ps2Simple {
public:
  virtual void doExtraAcquire() {
    load_teximages( getNext2k() * 2048 / 64 );
    sjoy_open();
  }
  virtual void doExtraRelease() {
    sjoy_close();
  }
};

class Eye {
public:
  bool stopped;
  float theta;
  float dtheta;
  float phi;

  void handleInput();
  void createViewingMatrix();
  void setViewPosition( ps2_vu0_fvector view );
};
  
void Eye::handleInput()
{
  const float pi = (float)M_PI;
		
  sjoy_poll();
  int paddata = sjoy_get_ps2_button(0);

  stopped = paddata & SJOY_PS2_R_RIGHT;

  if (paddata & SJOY_PS2_L_LEFT)
    theta += dtheta;
  else if (paddata & SJOY_PS2_L_RIGHT)
    theta -= dtheta;
		
  if (paddata & SJOY_PS2_L_UP)
    phi += dtheta;
  else if (paddata & SJOY_PS2_L_DOWN)
    phi -= dtheta;
		
  if (theta > 2.0f * pi)
    theta -= 2.0f * pi;
  else if (theta < 0.0f)
    theta += 2.0f * pi;
		
  if (phi >= 0.49f * pi)
    phi = 0.49f * pi;
  else if (phi <= -0.49f * pi)
    phi = -0.49f * pi;
}

void Eye::setViewPosition( ps2_vu0_fvector view )
{
  const float RADIUS = 65.0f;

  vset( view, 
	RADIUS * cosf(phi) * cosf(theta),
	RADIUS * sinf(phi),
	RADIUS * cosf(phi) * sinf(theta),
	1 );
}

void Eye::createViewingMatrix()
{
  const float g_fov = 42.0f;
  float screenOffset[16];
  
  ps2_vu0_fvector view;
  setViewPosition( view );

  ps2_gs_gparam *gp = ps2_gs_get_gparam();
  ps2_vu0_fvector viewVector, rightVector, tmpVector1;
  ps2_vu0_fvector tmpVector2;
	
  ps2_vu0_fvector interest;
  vset( interest, 0, .7, 0, 0 );
  ps2_vu0_sub_vector(viewVector, interest, view);
  float viewLength = length( viewVector );

  ps2_vu0_normalize(viewVector, viewVector);

  ps2_vu0_fvector upVector;
  vset( upVector, 0, 1, 0, 1 );
	
  ps2_vu0_outer_product(tmpVector1, viewVector, upVector);
  ps2_vu0_normalize(tmpVector1, tmpVector1);
  ps2_vu0_outer_product(upVector, tmpVector1, viewVector);
  ps2_vu0_outer_product(rightVector, upVector, viewVector);
  
  // ------ set offset for screen ------
  ps2_vu0_scale_vector(tmpVector2, rightVector, -1.0f);
  ps2_vu0_sub_vector(tmpVector1, tmpVector2, upVector);
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size);
  screenOffset[0] = tmpVector1[0];
  screenOffset[1] = tmpVector1[1];
  screenOffset[2] = tmpVector1[2];
  
  ps2_vu0_sub_vector(tmpVector1, upVector, rightVector);
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size);
  screenOffset[4] = tmpVector1[0];
  screenOffset[5] = tmpVector1[1];
  screenOffset[6] = tmpVector1[2];
  
  ps2_vu0_sub_vector(tmpVector1, rightVector, upVector);
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size);
  screenOffset[8] = tmpVector1[0];
  screenOffset[9] = tmpVector1[1];
  screenOffset[10] = tmpVector1[2];
  
  ps2_vu0_add_vector(tmpVector1, rightVector, upVector);
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size);
  screenOffset[12] = tmpVector1[0];
  screenOffset[13] = tmpVector1[1];
  screenOffset[14] = tmpVector1[2];
  
  ((u_int *)g_my_offset)[0] = *(u_int *)&(screenOffset[0]);
  ((u_int *)g_my_offset)[1] = *(u_int *)&(screenOffset[1]);
  ((u_int *)g_my_offset)[2] = *(u_int *)&(screenOffset[2]);
  
  ((u_int *)g_my_offset)[4] = *(u_int *)&(screenOffset[4]);
  ((u_int *)g_my_offset)[5] = *(u_int *)&(screenOffset[5]);
  ((u_int *)g_my_offset)[6] = *(u_int *)&(screenOffset[6]);
  
  ((u_int *)g_my_offset)[8] = *(u_int *)&(screenOffset[8]);
  ((u_int *)g_my_offset)[9] = *(u_int *)&(screenOffset[9]);
  ((u_int *)g_my_offset)[10] = *(u_int *)&(screenOffset[10]);
  
  ((u_int *)g_my_offset)[12] = *(u_int *)&(screenOffset[12]);
  ((u_int *)g_my_offset)[13] = *(u_int *)&(screenOffset[13]);
  ((u_int *)g_my_offset)[14] = *(u_int *)&(screenOffset[14]);
  
  // ========== set offset for ground =========
  
  vset( tmpVector1, 1, 0, 1, 0 );
  
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size_shadow);
  ((u_int *)g_my_shadow_offset)[0] = *(u_int *)&(tmpVector1[0]);
  ((u_int *)g_my_shadow_offset)[1] = *(u_int *)&(tmpVector1[1]);
  ((u_int *)g_my_shadow_offset)[2] = *(u_int *)&(tmpVector1[2]);
  
  tmpVector1[0] = -1.0f;
  tmpVector1[2] = 1.0f;
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size_shadow);
  ((u_int *)g_my_shadow_offset)[4] = *(u_int *)&(tmpVector1[0]);
  ((u_int *)g_my_shadow_offset)[5] = *(u_int *)&(tmpVector1[1]);
  ((u_int *)g_my_shadow_offset)[6] = *(u_int *)&(tmpVector1[2]);
  
  tmpVector1[0] = 1.0f;
  tmpVector1[2] = -1.0f;
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size_shadow);
  ((u_int *)g_my_shadow_offset)[8] = *(u_int *)&(tmpVector1[0]);
  ((u_int *)g_my_shadow_offset)[9] = *(u_int *)&(tmpVector1[1]);
  ((u_int *)g_my_shadow_offset)[10] = *(u_int *)&(tmpVector1[2]);
  
  tmpVector1[0] = -1.0f;
  tmpVector1[2] = -1.0f;
  ps2_vu0_scale_vector(tmpVector1, tmpVector1, g_part_size_shadow);
  ((u_int *)g_my_shadow_offset)[12] = *(u_int *)&(tmpVector1[0]);
  ((u_int *)g_my_shadow_offset)[13] = *(u_int *)&(tmpVector1[1]);
  ((u_int *)g_my_shadow_offset)[14] = *(u_int *)&(tmpVector1[2]);
  
  float fFOV = g_fov * (float)M_PI / 180.0f;
  
  float upLength = viewLength * sinf(fFOV * 0.5f) / cosf(fFOV * 0.5f);
  float rightLength = upLength * (float)gp->width / gp->height * gp->pixel_ratio;
  
  ps2_vu0_scale_vector(viewVector, viewVector, 1.0f / viewLength);
  ps2_vu0_scale_vector(upVector, upVector, 1.0f / upLength);
  ps2_vu0_scale_vector(rightVector, rightVector, 1.0f / rightLength);
  
  ps2_vu0_fmatrix viewMat;
  memset(viewMat, 0, sizeof(ps2_vu0_fmatrix));
  
  viewMat[0][0] = rightVector[0];
  viewMat[1][0] = rightVector[1];
  viewMat[2][0] = rightVector[2];
  
  viewMat[0][1] = upVector[0];
  viewMat[1][1] = upVector[1];
  viewMat[2][1] = upVector[2];
  
  viewMat[0][2] = viewVector[0];
  viewMat[1][2] = viewVector[1];
  viewMat[2][2] = viewVector[2];
  
  viewMat[3][3] = 1.0f;
  
  ps2_vu0_fmatrix transMat;
  memset(transMat, 0, sizeof(ps2_vu0_fmatrix));
  transMat[0][0] = 1.0f;
  transMat[1][1] = 1.0f;
  transMat[2][2] = 1.0f;
  transMat[3][3] = 1.0f;
  
  transMat[3][0] = - interest[0];
  transMat[3][1] = - interest[1];
  transMat[3][2] = - interest[2];
  
  ps2_vu0_fmatrix projMat;
  ps2_vu0_mul_matrix(projMat, transMat, viewMat);
  
  projMat[0][3] = projMat[0][2]; 
  projMat[1][3] = projMat[1][2]; 
  projMat[2][3] = projMat[2][2]; 
  
  ps2_vu0_fmatrix screenMat;
  screenMat[0][0] = gp->width / 2.0f;
  screenMat[1][0] = 0.0f;
  screenMat[2][0] = 0.0f;
  screenMat[3][0] = gp->width / 2.0f + gp->offset_x;
  
  screenMat[0][1] = 0.0f;
  screenMat[1][1] = -gp->height / 2.0f;
  screenMat[2][1] = 0.0f;
  screenMat[3][1] = gp->height / 2.0f + gp->offset_y;
  
  screenMat[0][2] = 0.0f;
  screenMat[1][2] = 0.0f;
  screenMat[2][2] = -100000.0f;
  screenMat[3][2] =  100000000.0f;
  
  screenMat[0][3] = 0.0f;
  screenMat[1][3] = 0.0f;
  screenMat[2][3] = 0.0f;
  screenMat[3][3] = 1.0f;
  
  ps2_vu0_fmatrix persMat;
  ps2_vu0_mul_matrix(persMat, screenMat, projMat);
  
  memcpy(g_my_matrix, persMat, sizeof(ps2_vu0_fmatrix));
  memcpy(g_my_shadow_matrix, persMat, sizeof(ps2_vu0_fmatrix));
  
  memcpy(My_ground_matrix, persMat, sizeof(ps2_vu0_fmatrix));
}

static bool ParseExtraOption( const char *option )
{
  if (strcmp( option, "-gas" ) == 0)
    g_fire = 0;
  else if ( strcmp( option, "-fire" ) == 0)
    g_fire = 1;
  else 
    return false;
  return true;
}

void InitParticleInfo()
{
  if (g_fire) {
    g_image_size = FIRE_IMAGE_SIZE;
    g_part_size = FIRE_PART_SIZE;
    g_part_size_shadow = FIRE_PART_SIZE_SHADOW;
    g_my_offset = My_offset_fire;
    g_my_shadow_offset = My_shadow_offset_fire;
    g_my_matrix = My_matrix_fire;
    g_my_shadow_matrix = My_shadow_matrix_fire;
    g_my_texture2 = My_texture2_fire;
    g_my_dma_start_part = My_dma_start_part_fire;
    g_my_dma_start_shadow_part = My_dma_start_shadow_part_fire;
    
    // init bound flags
    int n = FIRE_NUM_BLOCK * FIRE_NUM_PART * FIRE_NUM_EXPLODE;
    for (int i = 0; i < n; i++)
      fire_bound[i] = 0;
  }
  else {
    g_image_size = GAS_IMAGE_SIZE;
    g_part_size = GAS_PART_SIZE;
    g_part_size_shadow = GAS_PART_SIZE_SHADOW;
    g_my_offset = My_offset_gas;
    g_my_shadow_offset = My_shadow_offset_gas;
    g_my_matrix = My_matrix_gas;
    g_my_shadow_matrix = My_shadow_matrix_gas;
    g_my_texture2 = My_texture2_gas;
    g_my_dma_start_part = My_dma_start_part_gas;
    g_my_dma_start_shadow_part = My_dma_start_shadow_part_gas;
  }
}

int main(int argc, char *argv[])
{
  VPUFILE *vfd = vpuobj_open("vpu.elf", O_DATA_PS2MEM);
  if (vfd < 0) {
    perror("vpu_open");
    exit(1);
  }

  DemoPs2 Ps2;
  Ps2.init( argc, argv, ParseExtraOption, "[-gas] [-fire]" );

  InitParticleInfo();

  Eye eye;
  eye.theta = 0.0f;
  eye.phi = 0.0f;
  eye.dtheta = 2.0f * (float)M_PI / 180.0f;
	
  if (g_fire)
    SetParticlePositionFire(0);
  else
    SetParticlePositionGas(0);
	
  Ps2.start();
  for (;;) {
    eye.handleInput();

    int frame = Ps2.getFrame();
    // --- particle setup ---
    if (!eye.stopped)
      if (g_fire)
	SetParticlePositionFire(frame);
      else
	SetParticlePositionGas(frame);
  
    eye.createViewingMatrix();
		
    // ---- draw ground ----
    texenv2.draw( &g_img1, 6, 6, PS2_GS_SETREG_ALPHA(0, 2, 0, 1, 0));
    Ps2.dmaSendToVpu1( vfd, (ps2_dmatag *)My_dma_start);

    // ---- DRAW PARTICLE SHADOW ----
    texenv1.draw( &g_img3, 5, 5, 
		  g_fire ? PS2_GS_SETREG_ALPHA(0, 2, 0, 1, 0) : PS2_GS_SETREG_ALPHA(2, 0, 0, 1, 0));
    Ps2.dmaSendToVpu1( vfd, (ps2_dmatag *)g_my_dma_start_shadow_part);
		
    // ---- DRAW PARTICLE ----
    texenv1.draw( &g_img2, 5, 5, PS2_GS_SETREG_ALPHA(0, 2, 0, 1, 0));
    Ps2.dmaSendToVpu1( vfd, (ps2_dmatag *)g_my_dma_start_part);

    Ps2.flip();
  }
}