#include "CreatePolygon.h"

SchedDefineTask(CreatePolygon);

static void
ApplyMatrix(float *v, float *m)
{
    float t[4];

    t[0] = v[0];
    t[1] = v[1];
    t[2] = v[2];
    t[3] = v[3];

    for (int i = 0; i < 4; i++) {
	v[i] = t[0]*m[i] + t[1]*m[i+4] + t[2]*m[i+8] + t[3]*m[i+12];
    }
}

static void
ApplyNormalMatrix(float *v, float *m)
{
    float t[4];

    t[0] = v[0];
    t[1] = v[1];
    t[2] = v[2];

    for (int i = 0; i < 3; i++) {
        v[i] = t[0]*m[i] + t[1]*m[i+4] + t[2]*m[i+8];
    }
}


static int
run(SchedTask *smanager, void *rbuf, void *wbuf)
{

    TrianglePack *sg_tri = (TrianglePack*)smanager->get_input(0);
    texture_list *sg_texture_info = (texture_list*)smanager->get_input(1);
    float *sg_matrix = (float*)smanager->get_input(2);
    TrianglePack *pp_tri = (TrianglePack*)smanager->get_output(0);
    int *tri_num = (int*)smanager->get_param(0);

    float *matrix = sg_matrix;
    float *real_matrix = sg_matrix + 16;

    float xyz1[4], xyz2[4], xyz3[4];
    float normal1[4],normal2[4],normal3[4];

    for (int i = 0; i < *tri_num; i++) {

      TrianglePack *pp_cur_tri = &pp_tri[i];
      TrianglePack *sg_cur_tri = &sg_tri[i];
      
      xyz1[0] = sg_cur_tri->ver1.x;
      xyz1[1] = sg_cur_tri->ver1.y;
      xyz1[2] = sg_cur_tri->ver1.z*-1.0f;
      xyz1[3] = 1.0f;
      
      xyz2[0] = sg_cur_tri->ver2.x;
      xyz2[1] = sg_cur_tri->ver2.y;
      xyz2[2] = sg_cur_tri->ver2.z*-1.0f;
      xyz2[3] = 1.0f;
      
      xyz3[0] = sg_cur_tri->ver3.x;
      xyz3[1] = sg_cur_tri->ver3.y;
      xyz3[2] = sg_cur_tri->ver3.z*-1.0f;
      xyz3[3] = 1.0f;
    
      // matrix = 回転行列*透視変換行列
      ApplyMatrix(xyz1, matrix);
      ApplyMatrix(xyz2, matrix);
      ApplyMatrix(xyz3, matrix);
      
      xyz1[0] /= xyz1[2];
      xyz1[1] /= xyz1[2];
      xyz2[0] /= xyz2[2];
      xyz2[1] /= xyz2[2];
      xyz3[0] /= xyz3[2];
      xyz3[1] /= xyz3[2];
      
      pp_cur_tri->ver1.x = xyz1[0];
      pp_cur_tri->ver1.y = xyz1[1];
      pp_cur_tri->ver1.z = xyz1[2];
      pp_cur_tri->ver1.tex_x = sg_cur_tri->ver1.tex_x;
      pp_cur_tri->ver1.tex_y = sg_cur_tri->ver1.tex_y;
      
      pp_cur_tri->ver2.x = xyz2[0];
      pp_cur_tri->ver2.y = xyz2[1];
      pp_cur_tri->ver2.z = xyz2[2];
      pp_cur_tri->ver2.tex_x = sg_cur_tri->ver2.tex_x;
      pp_cur_tri->ver2.tex_y = sg_cur_tri->ver2.tex_y;
		
      pp_cur_tri->ver3.x = xyz3[0];
      pp_cur_tri->ver3.y = xyz3[1];
      pp_cur_tri->ver3.z = xyz3[2];
      pp_cur_tri->ver3.tex_x = sg_cur_tri->ver3.tex_x;
      pp_cur_tri->ver3.tex_y = sg_cur_tri->ver3.tex_y;
      
      normal1[0] = sg_cur_tri->normal1.x;
      normal1[1] = sg_cur_tri->normal1.y;
      normal1[2] = sg_cur_tri->normal1.z*-1.0f;
      normal1[3] = 0.0f;

      normal1[0] = sg_cur_tri->normal2.x;
      normal1[1] = sg_cur_tri->normal2.y;
      normal1[2] = sg_cur_tri->normal2.z*-1.0f;
      normal1[3] = 0.0f;

      normal1[0] = sg_cur_tri->normal3.x;
      normal1[1] = sg_cur_tri->normal3.y;
      normal1[2] = sg_cur_tri->normal3.z*-1.0f;
      normal1[3] = 0.0f;
    
      ApplyNormalMatrix(normal1,real_matrix);
      ApplyNormalMatrix(normal2,real_matrix);
      ApplyNormalMatrix(normal3,real_matrix);
      
      normal1[0] /= normal1[2];
      normal1[1] /= normal1[2];
      
      normal2[0] /= normal2[2];
      normal2[1] /= normal2[2];
      
      normal3[0] /= normal3[2];
      normal3[1] /= normal3[2];
      
      pp_cur_tri->normal1.x = normal1[0];
      pp_cur_tri->normal1.y = normal1[1];
      pp_cur_tri->normal1.z = normal1[2];
      
      pp_cur_tri->normal2.x = normal2[0];
      pp_cur_tri->normal2.y = normal2[1];
      pp_cur_tri->normal2.z = normal2[2];
      
      pp_cur_tri->normal3.x = normal3[0];
      pp_cur_tri->normal3.y = normal3[1];
      pp_cur_tri->normal3.z = normal3[2];
      
      pp_cur_tri->tex_info.addr   = sg_texture_info->pixels;
      pp_cur_tri->tex_info.width  = sg_texture_info->t_w;
      pp_cur_tri->tex_info.height = sg_texture_info->t_h;
      pp_cur_tri->tex_info.scale_max = sg_texture_info->scale_max;

    }
    
      return 0;

}