#include <math.h>
#include "SceneGraphRoot.h"
#include "SGList.h"

static void cube_move_left(SceneGraphPtr node, int screen_w, int screen_h);
static void cube_move_right(SceneGraphPtr node, int screen_w, int screen_h);
static void cube_move_idle(SceneGraphPtr node, int screen_w, int screen_h);
static void cube_collision(SceneGraphPtr node, int screen_w, int screen_h,
			   SceneGraphPtr tree);
static void cube_split(SceneGraphPtr root);

static void vacuum_move(SceneGraphPtr node, int w, int h);
static void vacuum_coll(SceneGraphPtr node, int w, int h);

static float vacuum_speed = 3.0f;

static void
cube_move_left(SceneGraphPtr node, int screen_w, int screen_h)
{
    node->xyz[0] -= node->stack_xyz[0];
#if 0
    node->xyz[1] -= node->stack_xyz[0] * node->stack_xyz[1];
    node->xyz[2] -= node->stack_xyz[0] * node->stack_xyz[2];
#else
    node->xyz[1] += node->stack_xyz[1];
    //node->xyz[2] -= node->stack_xyz[2];
#endif

    if (node->xyz[0] < 0) {
	node->set_move_collision(cube_move_right, cube_collision);
    }

    if (node->xyz[1] < 0 || node->xyz[1] > screen_h) {
	node->stack_xyz[1] = -node->stack_xyz[1];
    }

    //node->angle[0] += 2.0f;
    //node->angle[1] += 2.0f * node->stack_xyz[1];
    //node->angle[2] += 2.0f * node->stack_xyz[2];

    if (++node->frame > 60) {
	cube_split(node);
    }
}

static void
cube_move_right(SceneGraphPtr node, int screen_w, int screen_h)
{
    node->xyz[0] += node->stack_xyz[0];
#if 0
    node->xyz[1] -= node->stack_xyz[0] * node->stack_xyz[1];
    node->xyz[2] -= node->stack_xyz[0] * node->stack_xyz[2];
#else
    node->xyz[1] += node->stack_xyz[1];
#endif
    
    if (node->xyz[0] > screen_w) {
	node->set_move_collision(cube_move_left, cube_collision);
    }

    if (node->xyz[1] < 0 || node->xyz[1] > screen_h) {
	node->stack_xyz[1] = -node->stack_xyz[1];
    }

    //node->angle[0] += 2.0f;
    //node->angle[1] += 2.0f * node->stack_xyz[1];
    //node->angle[2] += 2.0f * node->stack_xyz[2];
    
    if (++node->frame > 60) {
	cube_split(node);
    }
}

static void
cube_split(SceneGraphPtr root)
{
    SceneGraphPtr p = root->clone();
    root->addBrother(p);

    root->set_move_collision(cube_move_left, cube_collision);
    p->set_move_collision(cube_move_right, cube_collision);

    root->frame = 0;
    p->frame = 0;

    p->xyz[0] = root->xyz[0] + 2;
    p->xyz[1] = root->xyz[1];
    p->xyz[2] = root->xyz[2];

    p->stack_xyz[0] = 2.0f;
    p->stack_xyz[1] = random()%3-1;
    p->stack_xyz[2] = random()%3-1;

    root->xyz[0] -= 2;
    root->stack_xyz[0] = 2.0f;
    root->stack_xyz[1] = random()%3-1;
    root->stack_xyz[2] = 0.0f;
    //root->stack_xyz[2] = random()%3-1;
}


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

    if (pad->circle.isPush()) {
	cube_split(node);
    }
}

static void
cube_collision(SceneGraphPtr node, int screen_w, int screen_h,
	       SceneGraphPtr tree)
{
}

static void
vacuum_coll(SceneGraphPtr node, int screen_w, int screen_h,
	    SceneGraphPtr tree)
{
    SceneGraphIteratorPtr it = sgroot->getIterator();
    SceneGraphPtr bigm;
    Pad *pad = sgroot->getController();
    float dx, dy, r;
    float q = 0;

    if (pad->cross.isRelease()) {
	return;
    }

    return;

    for (; it->hasNext(MCUBE);) {
	it->next(MCUBE);
	SceneGraphPtr mcube = it->get();
	
	dx = node->xyz[0] - mcube->xyz[0];
	dy = node->xyz[1] - mcube->xyz[1];
	
	r = sqrt(dx*dx + dy*dy);

	if (r >= 1) q = 300/r;

	if (r < 50.0f) {
	    mcube->remove();
	    continue;
	}

	if (dx == 0) {
	    if(mcube->xyz[1] > node->xyz[1]) {
		mcube->xyz[1] -= q;
	    } else if(mcube->xyz[1] < node->xyz[1]) {
		mcube->xyz[1] += q;
	    }
	} else {
	    if(mcube->xyz[0] > node->xyz[0]) {
		mcube->xyz[0] -= q*cos(atan(dy/dx));
		mcube->xyz[1] -= q*sin(atan(dy/dx));
		mcube->stack_xyz[0] = -q*cos(atan(dy/dx));
		mcube->stack_xyz[1] = -q*sin(atan(dy/dx));
	    } else if(mcube->xyz[0] < mcube->xyz[0]) {
		mcube->xyz[0] += q*cos(atan(dy/dx));
		mcube->xyz[1] += q*sin(atan(dy/dx));
		mcube->stack_xyz[0] = -q*cos(atan(dy/dx));
		mcube->stack_xyz[1] = -q*sin(atan(dy/dx));
	    }
	}
    }
}

static void
vacuum_move(SceneGraphPtr node , int w, int h)
{
    Pad *pad = sgroot->getController();

    if (pad->right.isHold()) {
	node->xyz[0] += vacuum_speed;
    } else if (pad->left.isHold()) {
	node->xyz[0] -= vacuum_speed;
    }

    if (pad->up.isHold()) {
	node->xyz[1] -= vacuum_speed;
    } else if (pad->down.isHold()) {
	node->xyz[1] += vacuum_speed;
    }

    if (pad->start.isPush()) {
	node->xyz[0] = w/2;
	node->xyz[1] = h*0.8;
    }
}

void
vacuum_init(int w, int h)
{
    SceneGraphPtr cube;
    SceneGraphPtr vacuum;
    SceneGraphPtr back;

    sgroot->createFromXMLfile("xml_file/mcube.xml");

    cube = sgroot->createSceneGraph(MCUBE);
    cube->xyz[0] = w/2;
    cube->xyz[1] = h/2;
    cube->xyz[2] = 0.0f;
    cube->set_move_collision(cube_move_idle, cube_collision);

    vacuum = sgroot->createSceneGraph(BIGMCUBE);
    vacuum->xyz[0] = w/2;
    vacuum->xyz[1] = h*0.8;
    vacuum->set_move_collision(vacuum_move, vacuum_coll);

    back = sgroot->createSceneGraph();
    back->addChild(vacuum);
    back->addChild(cube);

    sgroot->setSceneData(back);
}