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

#define FALSE 0
#define TRUE !FALSE
#define CHAIN_LEN 50

static double chain_width = 10;

typedef struct {
    double x, y, next_x, next_y;
    double vx, vy, next_vx, next_vy;
    double angle[3];
    int can_move;
    SceneGraphPtr parent;
    int id;
    //int parent;
} CHAIN_VARS;

/* SceneGraph の property */
CHAIN_VARS* properties[2];
CHAIN_VARS* property;


//void createSceneGraphFromProperty(CHAIN_VARS* p) ;
void createSceneGraphFromProperty(void* p) ;

void
init_chain_vars(CHAIN_VARS *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;
}

void
set_vector(CHAIN_VARS *p, SceneGraphPtr sg) {
    sg->xyz[0] = p->next_x;
    sg->xyz[1] = p->next_y;
    sg->xyz[2] = 0.0f;
    sg->angle[0] = p->angle[0];
    sg->angle[1] = p->angle[1];
    sg->angle[2] = p->angle[2];
}


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

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

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

void
chain_move(SceneGraphPtr sg, int w, int h)
{
    int id = sg->id;
    //CHAIN_VARS* p = (CHAIN_VARS*)sg->propertyptr;
    HTaskPtr chain_cal;
    CHAIN_VARS* output;

    // SceneGraph の切り替えもここでやる
    if (property == properties[0]) {
      property = properties[1];
      output   = properties[0];
    }else{
      property = properties[0];
      output   = properties[1];
    }
    chain_cal = manager->create_task(CHAINCAL_TASK);
    chain_cal->add_inData(property, sizeof(CHAIN_VARS)*CHAIN_LEN);
    chain_cal->add_param(id);
    chain_cal->add_outData(output, sizeof(CHAIN_VARS)*CHAIN_LEN);
    chain_cal->set_post(createSceneGraphFromProperty, (void*)id);	
    chain_cal->spawn();

}

void
chain_collision(SceneGraphPtr sg, int w, int h, SceneGraphPtr osg)
{

}

void 
createSceneGraphFromProperty(void* p) 
{
    CHAIN_VARS* chain_p = (CHAIN_VARS*)p;
    SceneGraphPtr chain_copy = sgroot->createSceneGraph(CHAIN);
    chain_copy->propertyptr = (void*)chain_p;
    chain_copy->property_size = sizeof(CHAIN_VARS);
    set_vector(chain_p, chain_copy);
    chain_p->parent->addChild(chain_copy);
}

void
chain_init(int w, int h)
{
    SceneGraphPtr root_chain, chain;
    CHAIN_VARS rcv;

    HTaskPtr chain_init;
    
    
    sgroot->createFromXMLfile("xml_file/chain.xml");

    /* SPE に送る property の配列の領域確保 */
    properties[0] = (CHAIN_VARS*)manager->allocate(sizeof(CHAIN_VARS)*CHAIN_LEN);
    properties[1] = (CHAIN_VARS*)manager->allocate(sizeof(CHAIN_VARS)*CHAIN_LEN);
    property = properties[0];

    root_chain = sgroot->createSceneGraph(CHAIN);
    // set_move_collision()ではだめ
    root_chain->set_move_collision(chain_move_ope, chain_collision);
    init_chain_vars(&rcv);
    rcv.next_x = w / 2;
    rcv.next_y = 0.0;
    rcv.angle[0] = 0;
    rcv.angle[1] = 0;
    rcv.angle[2] = 0;

    set_vector(&rcv, root_chain);

    for(int i = 0; i < CHAIN_LEN; i++) {
        chain = sgroot->createSceneGraph(CHAIN);
        property[i].id = i;
        init_chain_vars(&property[i]);
        property[i].x = 0;
        property[i].y = chain_width * i;
        set_vector(&property[i], chain);
        property->angle[1] = -90 * (i % 2);
        //chain->set_move_collision(chain_move, chain_collision);
	chain->propertyptr = &property[i];
	chain->property_size = sizeof(CHAIN_VARS);
        root_chain->addChild(chain);
	property[i].parent = root_chain;
    }
    property[0].can_move = FALSE;

    // property を SPU の共有領域へコピーする
    chain_init = manager->create_task(CHAININIT_TASK);
    chain_init->add_inData(property, sizeof(CHAIN_VARS)*CHAIN_LEN);
    chain_init->add_param(CHAIN_LEN);
    chain_init->set_cpu(SPE_0);
    chain_init->set_post(createSceneGraphFromProperty, (void*)property);
    chain_init->spawn();

    sgroot->setSceneData(root_chain);
}