#include <math.h>
#include "SceneGraphRoot.h"
#include "vacuum.h"
#define SELECT 2

void
cube_collision(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h,
               SceneGraphPtr tree)
{
    SceneGraphRoot *sgroot = (SceneGraphRoot *)sgroot_;
    if (node->frame > 120) {
	cube_split(node,tree, sgroot);
    }
}

void
cube_move_left(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h)
{
    node->xyz[0] -= node->stack_xyz[0];
    node->xyz[1] += node->stack_xyz[1];

    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];
    }
}

void
cube_rotate(SceneGraphPtr node, int w, int h)
{
    node->angle[0] += 2.0f;
    node->angle[1] += 2.0f;
    node->angle[2] += 2.0f;
}

void
cube_move_right(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h)
{
    node->xyz[0] += node->stack_xyz[0];
    node->xyz[1] += node->stack_xyz[1];

    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];
    }

}

extern int redcube ;
extern int enemy ;

void
cube_split(SceneGraphPtr root,SceneGraphPtr tree, SceneGraphRoot *sgroot
)
{

  SceneGraphPtr p;
  //  SceneGraphPtr common_move = sgroot->createSceneGraph();
  //  SceneGraphPtr root_common_move = root->parent;

  if(random()%SELECT) {
    p = sgroot->createSceneGraph(redcube);
  }
  else {
    p = sgroot->createSceneGraph(enemy);
  }

    root->set_move_collision(cube_move_right, cube_collision);
    p->set_move_collision(cube_move_left, 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] = 0.0f;

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

    //common_move->addChild(p);
    root->addBrother(p);

}


void
collision_red(SceneGraphIteratorPtr it,SceneGraphPtr node)
{
  float dx, dy,ddx,ddy, r;
  float q = 0;

  for (; it->hasNext(redcube);) {

    it->next(redcube);
    SceneGraphPtr mcube = it->get();
    dx = node->xyz[0] - mcube->xyz[0];
    dy = node->xyz[1] - mcube->xyz[1];

    ddx = dx*dx;
    ddy = dy*dy;

    if(sqrt(ddx) < 10 && sqrt(ddy) < 10) {
      mcube->remove();
      continue;
    }
    r = sqrt(ddx + ddy);
    if (r >= 1) q = 200/r;
    if (dx == 0) {
      if(mcube->xyz[1] > node->xyz[1]) {
        mcube->stack_xyz[1] -= q;
      } else if(mcube->xyz[1] < node->xyz[1]) {
        mcube->stack_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));
      } else if(mcube->xyz[0] < node->xyz[0]) {
        mcube->xyz[0] += q*cos(atan(dy/dx));
        mcube->xyz[1] += q*sin(atan(dy/dx));
      }
    }
  }
}

void
collision_purple(SceneGraphIteratorPtr it,SceneGraphPtr node,int w,int h, SceneGraphRoot *sgroot)
{
  float dx, dy,ddx,ddy, r;
  float q = 0;

  for (; it->hasNext(enemy);) {
    it->next(enemy);
    SceneGraphPtr mcube = it->get();

    dx = node->xyz[0] - mcube->xyz[0];
    dy = node->xyz[1] - mcube->xyz[1];
    ddx = dx*dx;
    ddy = dy*dy;

    if(sqrt(ddx) < 10 && sqrt(ddy) < 10) {
	gameover_scene(w,h,mcube, sgroot);
      node->remove();
      break;
    }
    r = sqrt(ddx + ddy);
    if (r >= 1) q = 200/r;
    if (dx == 0) {
      if(mcube->xyz[1] > node->xyz[1]) {
        mcube->stack_xyz[1] -= q;
      } else if(mcube->xyz[1] < node->xyz[1]) {
        mcube->stack_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));
      } else if(mcube->xyz[0] < node->xyz[0]) {
        mcube->xyz[0] += q*cos(atan(dy/dx));
        mcube->xyz[1] += q*sin(atan(dy/dx));
      }
    }
  }
}