#include <stdio.h>
#include "libps2.h"
#include "ps2util.h"
#include "col.h"
#include <stdbool.h>
#include <string.h>
#include <Viewer.h>

#define INIT_VECTOR(v,x,y,z,w) ((v)[0]=x,(v)[1]=y,(v)[2]=z,(v)[3]=w)
Viewer *droot;
void info_print( ColFace hd );

char *usr_help_str = "";
Pad *pad;

/**
 * 点pと面との衝突(点pとdetection face面との垂心oがdetection face領域の
 * 中にあるかを)判定衝突時(oがdetection face領域の外)はFALSEを戻し、そう
 * でなければTRUE、 垂心(orthocenter)、垂心のある面の法線(normal
 * vector)を戻す.
 */
int
TMmain(TaskManager *manager,int argc, char *argv[])
{
  droot = NULL;
  pad = NULL;
  printf("test_collision\n");
  ColFace colface;

  /*
  INIT_VECTOR(colface.p0, -14.4894953, 0.00938999932, 46.1109962, 0);
  INIT_VECTOR(colface.v0, 28.9789906, -0.0101119988, -49.6579971, 1);
  INIT_VECTOR(colface.v1, 0, -0.0101119988, -49.6579971, 0);
  INIT_VECTOR(colface.normal, 0, -1, -0.000203632837, 1);
  */
  INIT_VECTOR(colface.p0, 11.0305262, -0.00100499997, -4.93506002, 0);
  INIT_VECTOR(colface.v0, -22.2344475, 0.0162969995, 80.0279922, 1);
  INIT_VECTOR(colface.v1, 0, 0.0162969995, 80.0279922, 0);
  INIT_VECTOR(colface.normal, 0, -0.99999994, -0.000203641233, 1);

  info_print( colface );

  FVECTOR vertical = {0,-1,0,1};
  FVECTOR location;

  ColFaceHd hd;
  hd.colface = &colface;
  hd.facenum = 1;

  if ( col_detect(&hd, vertical, location) == true ) {
    printf("true"); // コース上
  }
  else{
    printf("false");
  }
  printf("\n");
  return 0;
}

void info_print( ColFace hd )
{
  printf("hd.p0 = {");
  for( int i = 0; i < 4; i ++ ){
    printf(" %f,",hd.p0[i]);
  }
  printf("}\n");

  
  printf("hd.v0 = {");
  for( int i = 0; i < 4; i ++ ){
    printf(" %f,",hd.v0[i]);
  }
  printf("}\n");


  printf("hd.v1 = {");
  for( int i = 0; i < 4; i ++ ){
    printf(" %f,",hd.v1[i]);
  }
  printf("}\n");

  printf("hd.cormal = {");
  for( int i = 0; i < 4; i ++ ){
    printf(" %f,",hd.normal[i]);
  }
  printf("}\n");
}