#include <math.h>
#include <stdlib.h>
#include "SceneGraphRoot.h"
#include "matrix_calc.h"
#include "matrix_test.h"

/*
 * ある頂点と、その法線を対象に、それぞれの座標系で光源計算をするテストコード
 *
*/

static void
print_vector4(const float *obj_re_v, const float *light_re_v,
             const float *light_vector, const float *nor_re_v)

{

    printf("obj_v");
    for (int i = 0; i < 4; i++) printf(" %f",obj_re_v[i]);
    printf("\n");

    printf("light_v");
    for (int i = 0; i < 4; i++) printf(" %f",light_re_v[i]);
    printf("\n");

    printf("light_vector");
    for (int i = 0; i < 4; i++) printf(" %f",light_vector[i]);
    printf("\n");

    printf("normal_vector");
    for (int i = 0; i < 4; i++) printf(" %f",nor_re_v[i]);
    printf("\n");
    printf("\n");


}

static void
reject_translation(float *m_out, const float *m_in)
{

    for (int i = 0; i < 16; i++) m_out[i] = m_in[i];
    m_out[4*0+3]=m_out[4*1+3]=m_out[4*2+3]=0;
 
}



MainLoopPtr 
matrix_test::init(Viewer *sgroot, int screen_w, int screen_h)
{

    CameraPtr ca = sgroot->getCamera();

    SceneGraphPtr light = sgroot->getLight(0);


    light->xyz[0] = screen_w/2;
    light->xyz[1] = screen_h/2;
    light->xyz[2] = 10;

    float light_v[4] = {0,0,0,1}; //ローカル座標

    sgroot->createFromXMLfile("xml_file/panel_512.xml");
    SceneGraphPtr obj = sgroot->createSceneGraph("PANEL_512");
    obj->xyz[0] = screen_w/2;
    obj->xyz[1] = screen_h/2;
    obj->xyz[2] = 100;

    //obj->angle[0] = 0;

    PolygonPack *pp = obj->pp;

    int polygon_size = pp->info.size;
    printf("polygon size %d \n",polygon_size);

    VertexPack v = obj->pp->tri[0].ver1;
    float obj_v[4];

    obj_v[0] = v.x;
    obj_v[1] = v.y;
    obj_v[2] = v.z;
    obj_v[3] = 1.0f;

    printf("ver x %f\n",v.x);
    printf("ver y %f\n",v.y);
    printf("ver z %f\n",v.z);

    NormalPack n = obj->pp->tri[0].normal1;
    float normal[4];
    normal[0] = n.x;
    normal[1] = n.y;
    normal[2] = n.z;
    normal[3] = 0.0f;

    printf("nor x %f\n",n.x);
    printf("nor y %f\n",n.y);
    printf("nor z %f\n",n.z);
    printf("\n\n");

    float obj_re_v[4];
    float light_re_v[4];
    float nor_re_v[4];
    float light_vector[4];
    float normal_matrix[16];

    /* 
     *
     * --world--
     *
     */

    printf("--world--\n");

    float unit[16];
    unitMatrix(unit);

    // wrold matrix * unit matrix
    get_matrix(obj->matrix, obj->angle, obj->xyz, unit);
    get_matrix(light->matrix, light->angle, light->xyz, unit);
    
    // vertex * matrix
    applyMatrix(obj_re_v,   obj->matrix,   obj_v);
    applyMatrix(light_re_v, light->matrix, light_v);

    // make normal matrix
    reject_translation(normal_matrix, obj->matrix);

    light_vector[0] = obj_re_v[0] - light_re_v[0];
    light_vector[1] = obj_re_v[1] - light_re_v[1];
    light_vector[2] = obj_re_v[2] - light_re_v[2];

    applyMatrix(nor_re_v, obj->matrix, normal);

    normalize(light_vector, light_vector);
    normalize(nor_re_v, nor_re_v);

    float p = innerProduct(light_vector, nor_re_v);

    printf("inner Product %f\n", p);
    printf("\n");
    printf("\n");

    print_vector4(obj_re_v, light_re_v, light_vector, nor_re_v);

    /* 
     *
     * --world * view--
     *
     */

    printf("--world * view--\n");

    // world matrix * view matrix
    get_matrix(obj->matrix, obj->angle, obj->xyz, ca->m_view);
    get_matrix(light->matrix, light->angle, light->xyz, ca->m_view);

    // vertex * matrix
    applyMatrix(obj_re_v,   obj->matrix,   obj_v);
    applyMatrix(light_re_v, light->matrix, light_v);

    // make normal matrix
    reject_translation(normal_matrix, obj->matrix);

    applyMatrix(nor_re_v, obj->matrix, normal);

    light_vector[0] = obj_re_v[0] - light_re_v[0];
    light_vector[1] = obj_re_v[1] - light_re_v[1];
    light_vector[2] = obj_re_v[2] - light_re_v[2];

    normalize(light_vector, light_vector);
    normalize(nor_re_v, nor_re_v);

    p = innerProduct(light_vector, nor_re_v);
    printf("inner Product %f\n", p);

    print_vector4(obj_re_v, light_re_v, light_vector, nor_re_v);

    /* 
     *
     * --world * view * perspective--
     *
     */

    printf("--world * view * perspective--\n");
    mulMatrix4x4(obj->matrix,   ca->m_pers);    
    mulMatrix4x4(light->matrix, ca->m_pers);

    applyMatrix(obj_re_v,   obj->matrix,   obj_v);
    applyMatrix(light_re_v, light->matrix, light_v);

    // make normal matrix
    reject_translation(normal_matrix, obj->matrix);

    applyMatrix(nor_re_v, obj->matrix, normal);

    obj_re_v[0] /= obj_re_v[3];
    obj_re_v[1] /= obj_re_v[3];
    obj_re_v[2] /= obj_re_v[3];

    light_re_v[0] /= light_re_v[3];
    light_re_v[1] /= light_re_v[3];
    light_re_v[2] /= light_re_v[3];

    light_vector[0] = obj_re_v[0] - light_re_v[0];
    light_vector[1] = obj_re_v[1] - light_re_v[1];
    light_vector[2] = obj_re_v[2] - light_re_v[2];

    normalize(light_vector, light_vector);
    normalize(nor_re_v, nor_re_v);

    p = innerProduct(light_vector, nor_re_v);
    printf("inner Product %f\n", p);

    print_vector4(obj_re_v, light_re_v, light_vector, nor_re_v);

    /* 
     *
     * --world * view * perspective * screen--
     *
     */

    printf("--world * view * perspective * screen--\n");
    mulMatrix4x4(obj->matrix,  ca->m_screen);
    mulMatrix4x4(light->matrix, ca->m_screen);

    applyMatrix(obj_re_v,   obj->matrix,   obj_v);
    applyMatrix(light_re_v, light->matrix, light_v);

    // make normal matrix
    reject_translation(normal_matrix, obj->matrix);

    applyMatrix(nor_re_v, obj->matrix, normal);

    obj_re_v[0] /= obj_re_v[3];
    obj_re_v[1] /= obj_re_v[3];
    obj_re_v[2] /= obj_re_v[3];

    light_re_v[0] /= light_re_v[3];
    light_re_v[1] /= light_re_v[3];
    light_re_v[2] /= light_re_v[3];

    light_vector[0] = obj_re_v[0] - light_re_v[0];
    light_vector[1] = obj_re_v[1] - light_re_v[1];
    light_vector[2] = obj_re_v[2] - light_re_v[2];

    normalize(light_vector, light_vector);
    normalize(nor_re_v, nor_re_v);

    p = innerProduct(light_vector, nor_re_v);
    printf("inner Product %f\n", p);

    print_vector4(obj_re_v, light_re_v, light_vector, nor_re_v);


    /* 
     *
     * --Cerium--
     *
     */

    printf("--Cerium--\n");

    get_matrix(obj->matrix, obj->angle, obj->xyz, ca->matrix);
    get_matrix(light->matrix, light->angle, light->xyz, ca->matrix);

    applyMatrix(obj_re_v,   obj->matrix,   obj_v);
    applyMatrix(light_re_v, light->matrix, light_v);

    // make normal matrix
    reject_translation(normal_matrix, obj->matrix);

    applyMatrix(nor_re_v, obj->matrix, normal);

    obj_re_v[0] /= obj_re_v[3];
    obj_re_v[1] /= obj_re_v[3];
    obj_re_v[2] /= obj_re_v[3];

    light_re_v[0] /= light_re_v[3];
    light_re_v[1] /= light_re_v[3];
    light_re_v[2] /= light_re_v[3];

    light_vector[0] = obj_re_v[0] - light_re_v[0];
    light_vector[1] = obj_re_v[1] - light_re_v[1];
    light_vector[2] = obj_re_v[2] - light_re_v[2];

    normalize(light_vector, light_vector);
    normalize(nor_re_v, nor_re_v);

    p = innerProduct(light_vector, nor_re_v);
    printf("inner Product %f\n", p);

    print_vector4(obj_re_v, light_re_v, light_vector, nor_re_v);

    sgroot->setSceneData(obj);


    return sgroot;
}

extern Application *
application() {
    return new matrix_test();
}

const char *usr_help_str = "Usage: ./test_nogl [OPTION]\n";

extern int init(TaskManager *manager, int argc, char *argv[]);
extern void task_initialize();
static void TMend(TaskManager *manager);

int
TMmain(TaskManager *manager, int argc, char *argv[])
{
    task_initialize();
    manager->set_TMend(TMend);
    return init(manager, argc, argv);

}

void
TMend(TaskManager *manager)
{
    printf("test_nogl end\n");
}

/* end */