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

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

void
cube_move_left(SceneGraphPtr node, 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, 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];
    }

}


void
cube_split(SceneGraphPtr root,SceneGraphPtr tree)
{

  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)
{
  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);
      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));
      }
    }
  }
}