#include <math.h>
#include <stdlib.h>
#include <unistd.h>
#include "SceneGraphRoot.h"
#include "MainLoop.h"
#include "motion_man.h"


float x=0,y=-80;
float dx=0,dy=80;
float tx=0,ty=10;
float cx=0,cy=-10;
static int cube_subID,cub5ID,cub6ID;
static float cube_radius = 35.0f;
static float cube_sub_radius = 35.0f;

static void
push_move_false(SceneGraphPtr, void *sgroot_, int w, int h, SceneGraphPtr tree) {
}

static void
walk(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h, SceneGraphPtr tree) {
	SceneGraphRoot *sgroot = (SceneGraphRoot *)sgroot_;
	SceneGraphIteratorPtr it = sgroot->getIterator(tree);

	Pad *pad = sgroot->getController();
	it->next(cub5ID);
	SceneGraphPtr cub5 = it->get();

	if (pad->left.isHold()) {
		if (x==80) { 
			cub5->angle[0] -= 10; 
			y=cub5->angle[0];
			if (y==-80)
				x=0;
		} else if (y==-80) {
			cub5->angle[0] += 10;
			x=cub5->angle[0];
		}
	}

	it->next(cub6ID);
	SceneGraphPtr cub6 = it->get();
	if (pad->left.isHold()) {
		 if (dx==-80) { 
			cub6->angle[0] += 10; 
			dy=cub6->angle[0];
			if (dy==80)
				dx=0;
		} else if (dy==80) {
			cub6->angle[0] -= 10;
			dx=cub6->angle[0];
		}
	} 

	float x_distance,y_distance,distance;
	it->next(cube_subID);
	SceneGraphPtr cube_sub = it->get();
	x_distance = node->xyz[0] - cube_sub->xyz[0];
	y_distance = node->xyz[1] - cube_sub->xyz[1];

	distance = hypotf(x_distance,y_distance);

	if (distance <= (cube_radius = cube_sub_radius)) {
		cube_sub->xyz[0] -= 30;
		if (cube_sub->xyz[0]<-40)
			cube_sub->xyz[0] = screen_w / 3;
	}
}

static void
move_left(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h) {

	SceneGraphRoot *sgroot = (SceneGraphRoot *)sgroot_;
	Pad *pad = sgroot->getController();

	if (pad->left.isHold()) {
		node->xyz[0] -= 10;	 
		if (node->xyz[0] < 0)
			node->xyz[0] = screen_w;
	}
}

static void
push_right_arm(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h) {

	SceneGraphRoot *sgroot = (SceneGraphRoot *)sgroot_;
	Pad *pad = sgroot->getController();
	
	if (pad->triangle.isHold()) {
		if (tx==-80) { 
			node->angle[1] += 10; 
			ty=node->angle[1];
			if (ty==10)
				tx=0;
		} else if (ty==10) {
			node->angle[1] -= 10;
			tx=node->angle[1];
		}
	} 
}

static void
push_left_arm(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h) {

	SceneGraphRoot *sgroot = (SceneGraphRoot *)sgroot_;
	Pad *pad = sgroot->getController();
	
	if (pad->square.isHold()) {
		if (cx==80) { 
			node->angle[1] -= 10; 
			cy=node->angle[1];
			if (cy==-10)
				cx=0;
		} else if (cy==-10) {
			node->angle[1] += 10;
			cx=node->angle[1];
		}
	} 
}

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

    SceneGraphPtr cube1;
    SceneGraphPtr cube2;
    SceneGraphPtr cube3;
    SceneGraphPtr cube4;
    SceneGraphPtr cube5;
    SceneGraphPtr cube6;
    
    sgroot->createFromXMLfile("xml_file/parent.xml");

    SceneGraphPtr light = sgroot->getLight(0);
    sgroot->OnLightSwitch(0);
    light->xyz[0] = screen_w / 2;
    light->xyz[1] = screen_h / 2;
    light->xyz[2] = -100;

	SceneGraphPtr root = sgroot->createSceneGraph();
    cube1 = sgroot->createSceneGraph("Cube");

    cube1->xyz[0] = (screen_w / 2) / 2 + (screen_w / 2);
    cube1->xyz[1] = screen_h / 2;
    cube1->xyz[2] = 0;
	cube1->angle[1] = 90;

	root->addChild(cube1);
	
    cube2 = sgroot->createSceneGraph("Cube.001");
    cube3 = sgroot->createSceneGraph("Cube.002");
    cube4 = sgroot->createSceneGraph("Cube.003");
    cube5 = sgroot->createSceneGraph("Cube.004");
	cub5ID = cube5->sgid;
    cube6 = sgroot->createSceneGraph("Cube.005");
	cub6ID = cube6->sgid;
    cube1->addChild(cube2);
    cube1->addChild(cube3);
    cube1->addChild(cube4);
    cube1->addChild(cube5);
    cube1->addChild(cube6);

    cube1->set_move_collision(move_left, walk);
	cube3->set_move_collision(push_right_arm, push_move_false);
	cube2->set_move_collision(push_left_arm, push_move_false);

    SceneGraphPtr cube_sub = sgroot->createSceneGraph("Cube");
	cube_subID = cube_sub->sgid;
	cube_sub->xyz[0] = screen_w / 3;
	cube_sub->xyz[1] = screen_h / 2;
	cube_sub->xyz[2] = 0;
	root->addChild(cube_sub);

    sgroot->setSceneData(root);

    return sgroot;
}

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

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