#include <iostream>
#include <math.h>
#include "SceneGraphRoot.h"
#include "SceneGraph.h"
#include "SGList.h"
#include "TaskManager.h"
#include "Func.h"
#include "Chain.h"
#define FALSE 0
#define TRUE !FALSE
#define CHAIN_LEN 50

static double chain_width = 10;


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


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

void
Chain::init_chain_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;
}

void
set_vector(ChainPropertyPtr 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];
}

#if 0
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;
    }
}
#endif

void
Chain::chain_move(TaskManager *manager, SceneGraphPtr sg, int w, int h)
{
    int id = sg->id;
    //ChainPropertyPtr p = (ChainPropertyPtr)sg->propertyptr;
    HTaskPtr chain_cal;
    ChainPropertyPtr 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(ChainProperty)*CHAIN_LEN);
    chain_cal->add_param(id);
    chain_cal->add_outData(output, sizeof(ChainProperty)*CHAIN_LEN);
    chain_cal->set_post(createSceneGraphFromProperty, (void*)id);
    chain_cal->spawn();

}

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

}

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

void
Chain::init(TaskManager *manager, int w, int h)
{
    SceneGraphPtr root_chain, chain;
    ChainPropertyPtr rcv;
	ChainProperty r;
    HTaskPtr chain_init;

	rcv = &r;    
    
    sgroot->createFromXMLfile(manager, "xml_file/chain.xml");

    /* SPE に送る property の配列の領域確保 */
    properties[0] = (ChainPropertyPtr)manager->allocate(sizeof(ChainProperty)*CHAIN_LEN);
    properties[1] = (ChainPropertyPtr)manager->allocate(sizeof(ChainProperty)*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(ChainProperty);
        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(ChainProperty)*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);
}