#include <iostream>
#include <math.h>
#include "SceneGraphRoot.h"
#include "SceneGraph.h"
#include "TaskManager.h"
#include "property_test.h"
#include "Func.h"

static ChainPropertyPtr properties[2];
static ChainProperty cv[CHAIN_LEN];


static void
init_chainold_vars(ChainPropertyPtr cv) {
    cv->x = 0, cv->y = 0, cv->next_x = 0, cv->next_y = 0;
    cv->vx = 0, cv->vy = 0, cv->next_vx = 0, cv->next_vy = 0;
    cv->can_move = TRUE;
}

static void
set_old_vector(ChainPropertyPtr cv, SceneGraphPtr sg) {
    sg->xyz[0] = (float)cv->next_x;
    sg->xyz[1] = (float)cv->next_y;
    sg->xyz[2] = 0.0f;
}


static void
chain_old_move_ope(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h)
{
    SceneGraphRoot *sgroot = (SceneGraphRoot *)sgroot_;
    Pad *pad = sgroot->getController();

    if (pad->cross.isHold()) {
        cv[CHAIN_LEN-1].can_move = FALSE;
        if (pad->left.isHold()) {
            cv[CHAIN_LEN-1].x += -5.0;
        } else if (pad->right.isHold()) {
            cv[CHAIN_LEN-1].x += 5.0;
        }

        if (pad->up.isHold()) {
            cv[CHAIN_LEN-1].y += -5.0;
        } else if (pad->down.isHold()) {
            cv[CHAIN_LEN-1].y += 5.0;
        }
    } else {
        cv[CHAIN_LEN-1].can_move = TRUE;
    }
}

static void
chain_old_move(SceneGraphPtr sg, void *sgroot_, int w, int h)
{
    int id = sg->id;
    if(id == 0) {
        for(int cnt = 0; cnt < 600; cnt++) {
            for(int i = 0; i < CHAIN_LEN; i++) {
                if(cv[i].can_move) {
                    double dx = cv[i-1].x - cv[i].x;
                    double dy = cv[i-1].y - cv[i].y;
                    double l = sqrt(dx * dx + dy * dy);
                    double a = k * (l - chain_width) / m;
                    double ax = a * dx / l;
                    double ay = a * dy / l;
                    if(i < CHAIN_LEN - 1) {
                        dx = cv[i+1].x - cv[i].x;
                        dy = cv[i+1].y - cv[i].y;
                        l = sqrt(dx * dx + dy * dy);
                        a = k * (l - chain_width) / m;
                        ax += a * dx / l;
                        ay += a * dy / l;
                    }
                    ay += g;
                    cv[i].vx *= safe;
                    cv[i].vy *= safe;
                    cv[i].next_vx = cv[i].vx + ax * dt;
                    cv[i].next_vy = cv[i].vy + ay * dt;
                    cv[i].next_x = cv[i].x + cv[i].vx * dt;
                    cv[i].next_y = cv[i].y + cv[i].vy * dt;
                } else {
                    cv[i].next_x = cv[i].x;
                    cv[i].next_y = cv[i].y;
                }
            }
            for(int i = 0; i < CHAIN_LEN; i++) {
                cv[i].vx = cv[i].next_vx;
                cv[i].vy = cv[i].next_vy;
                cv[i].x = cv[i].next_x;
                cv[i].y = cv[i].next_y;
            }
        }
        //    cout << id << ", " << sg->xyz[1] << endl;
    }
    set_old_vector(&cv[id], sg);
    int p, n;
    p = n = id;
    if(p != 0) {
        p--;
    }
    if(n != CHAIN_LEN - 1) {
        n++;
    }
    sg->angle[2-(id%2)*2]
        = 90 + atan((cv[p].next_y - cv[n].next_y) / (cv[p].next_x - cv[n].next_x)) * 180 / M_PI;
}



static void
chain_old_collision(SceneGraphPtr node, void *sgroot_, int w, int h, SceneGraphPtr ocv)
{
    //createSceneGraphFromProperty(cv, cv);
}



static void
createSceneGraphFromProperty(SceneGraphPtr root, ChainPropertyPtr cv, Viewer *sgroot)
{
    SceneGraphPtr node;

    for(int i = 0; i < CHAIN_LEN; i++) {
        node = sgroot->createSceneGraph(cv[i].objname);
        /**
         * move, collision は spe で実行される, task taskID を持たせればよい
         * property が 対応する taskID を持つので set_move_collision() は不要
         * set_task_propety(move_taskID, collision_taskID) のようなものを作る
         */
        node->set_move_collision(chain_old_move, chain_old_collision);
        //spe_move_collision(cv[i], chain_old_move, chain_old_collision);
        node->id = cv[i].id;
        node->angle[1] = cv[i].angle[1];
        set_old_vector(&cv[i], node);
        cv[i].parent->addChild(node);
    }
    sgroot->setSceneData(root);
}


static void
set_properties(ChainPropertyPtr cv)
{
    properties[0] = cv;
    properties[1] = cv;
}

MainLoopPtr
Chain::init(Viewer *sgroot, int w, int h)
{
    SceneGraphPtr root, chain;
    sgroot->createFromXMLfile("xml_file/chain.xml");

    ChainProperty rcv;
    init_chainold_vars(&rcv);
    rcv.next_x = w / 2;
    rcv.next_y = 0.0;
    rcv.id = CHAIN_LEN;

    root = sgroot->createSceneGraph("CHAIN");
    root->set_move_collision(chain_old_move_ope, chain_old_collision);
    set_old_vector(&rcv, root);

    chain = sgroot->createSceneGraph("CHAIN");
    chain->set_move_collision(chain_old_move, chain_old_collision);

    set_properties(cv);

    for(int i = 0; i < CHAIN_LEN; i++) {
        init_chainold_vars(&cv[i]);
        cv[i].x = 0;
        cv[i].y = chain_width * i;
        cv[i].angle[1] = -90 * (i % 2);

        chain->id = cv[i].id;
        chain->angle[1] = cv[i].angle[1];
        set_old_vector(&cv[i], chain);

        cv[i].root = root;
        cv[i].objname = "CHAIN";
        cv[i].id = i;

        cv[i].parent = root;

    }

    createSceneGraphFromProperty(root, cv, sgroot);
    cv[0].can_move = FALSE;
    //sgroot->setSceneData(root);

    return sgroot;
}

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

extern 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 */