#include "type.h"

drrt::drrt(){
  head = new treenode(0,0);
  goal = NULL;
  oldhead = NULL;
  }

drrt::~drrt(){
  treenode * temp;
  forall(temp, deadnodes){
    allnodes.push(deadnodes.pop());
    }
  forall(temp, allnodes){
    delete temp;
    }
  }

drrt::drrt(double x, double y){
  head = new treenode(x,y);
  goal = NULL;
  oldhead = NULL;
  }

drrt::drrt(treenode * tn){
  head = tn;
  goal = NULL;
  oldhead = NULL;
  }

//drrt::drrt(const & drrt){}

void drrt::initialize(){
  treenode * temp;
  allnodes.push(head);
  for(int i=1;i<DRRT_SIZE;i++){
    temp = new treenode();
    deadnodes.push(temp);
    }
  makeundead();
  goal = findgoal(E.predictedx,E.predictedy);
//  logfile << "initialized" << endl;
  }

void drrt::addnode(){
cout << "addnode" <<endl;
  treenode * add;
  treenode *temp;
  double a, b;
  double dist, metric;
  double min = 1000000, metricmin = 1000000;  
  double chance;
  bool collide=false;
  obstacle obs;
  polygon poly, tpoly;
  point pt,pt1,pt2;

  int r,g,bl;

  treenode * closest = allnodes.front();

cout << "1" <<endl;
  rand >> chance;
  chance*=100.0;
  if (chance<5.0){
    a = E.predictedx; b = E.predictedy;
    }
//  else{
//    rand>>a;rand>>b;a*=100;b*=100;
//    }
  else{
    rand >> a; rand >> b; a*=100; b*=100; 
    }

cout << "2" << endl;

  forall(temp, allnodes){
    dist = (a - temp->x)*(a - temp->x)+(b - temp->y)*(b - temp->y);
    metric = dist + temp->count;
//((temp->count*.25)*(temp->count*.25));
    if(metric < metricmin){
      closest = temp;
      metricmin = metric;
      min = dist;
      }
    }
  
cout << "3" << endl;

  a -= closest->x;
  b -= closest->y;

  min = sqrt(min);
  
  if(min==0){
    a=0;
    b=0;
    }
  else{
    a /= min;
    b /= min;
    }

  a *= STEP;
  b *= STEP;

  a += closest->x;
  b += closest->y;
  
  pt = point(a,b);
  tpoly = head->body.translate(pt.xcoord(),pt.ycoord());
  
//  if(chance > 50.0){
//    a = closest->x;
//    b = closest->y;
//    }

  collide = false;
  forall(obs, O){
    poly = obs.body((double)closest->count+1);
    forall_vertices(pt1,tpoly){
      if(poly.inside(pt1)){
        collide = true;
        break;
        }
      }
    if(collide){
      break;
      } 
    forall_vertices(pt2,poly){
      if(tpoly.inside(pt2)){
        collide = true;
        break;
        }
      }
    if(collide){
      break;
      }
    }

  if(!collide){
    total_nodes++;
    add = deadnodes.pop();
    add->x = a;
    add->y = b;
    add->branches.clear();
    add->parent=closest;
    add->count=closest->count+1;

    r = 0x00FF0000 & add->count;
    g = 0x0000FF00 & add->count;
    bl = 0x000000FF & add->count;   

    W.draw_segment(closest->x,closest->y,add->x,add->y,color(r,g,bl));
    PS.draw_segment(closest->x,closest->y,add->x,add->y,color(r,g,bl));

    closest->branches.push(add);
    allnodes.push(add);
//    logfile << "Added a new spot on the tree!" << endl;
    }
  else{
//    logfile << "Collided w/tree, trying again!" << endl;
    }
cout << "exit addnode" << endl;
  }

treenode * drrt::nextnode(double a, double b){
cout << "nextnode" <<endl;
  treenode * temp;
  goal = findgoal(a, b);
  cout << "goal" << endl;

  path.clear();

  cout << "clear" << endl;

  if(head == goal){
    return(NULL);
    }

  if(goal){
    cout << "head" << endl;
    }
  else{ 
    cout << "dead head" << endl;
    }
 
  for(temp = goal;temp->parent!=head;temp=temp->parent){
  cout << "bush" << endl;
    path.push(temp);
  cout << "nottheband" << endl;
  if(temp){
    cout <<"yes" <<endl;
    }
  else{
    cout << "no" << endl;
    }
    }

  cout << "ass" <<endl;

  path.push(temp);

cout << "Found next node!" << endl;
  return(temp);
  }

treenode * drrt::findgoal(double a, double b){
cout << "findgoal" << endl;
  treenode * temp = NULL;
  treenode * closest = NULL;
  double min = 10000000;
  double dist;

  closest = allnodes.front();

  forall(temp, allnodes){
    dist = (E.predictedx - temp->x)*(E.predictedx - temp->x)+
           (E.predictedy - temp->y)*(E.predictedy - temp->y);

    if(dist < min){
      if(closest){
        if(min < 1){
          if(closest->count > temp->count){
            closest = temp;
            min = dist;
            }
          }
        else{
          closest = temp;
          min = dist;
          }
        }
      else{
        closest = temp;
        min = dist;
        }
      }
    }

if(closest){
  cout << "closest found" << endl;
  }
else{
  cout << "no close node was found"  << endl;
  cout << "size of allnodes is " << allnodes.size() << endl;
  }

cout << "Found the goal!" << endl;
  return(closest);
  }

void drrt::growtree(){
cout << "growtree" << endl;
  treenode * temp;
  for(int i=0; i<INC_SIZE;i++){
    temp = new treenode();
    deadnodes.push(temp);
    }
  makeundead();
  } 

bool drrt::move(){
cout << "move" << endl;
  treenode * next;
  treenode * temp;
  treenode * middle;
  oldhead = middle = head;
  polygon tpoly;
  point pt;
  E.predict();
  next = nextnode(E.predictedx, E.predictedy);
  while(next==NULL){
    growtree();
    if(allnodes.size()+deadnodes.size()>max_mem){
      max_mem = allnodes.size()+deadnodes.size();
      }
    next = nextnode(E.predictedx,E.predictedy);
    }
  tpoly = head->body.translate(head->x, head->y);
//  for(int i=0; i<MAX_STEPS && path.size()>0;i++){
    middle = head;
    head = path.pop();
    W.draw_filled_polygon(tpoly,color(255,0,0));
    PS.draw_filled_polygon(tpoly,color(255,0,0));

//    W.draw_segment(middle->x,middle->y,head->x,head->y,color(255,0,0));
//    }

  pt = point(E.getx(),E.gety());
  if(tpoly.inside(pt)){
    tree_length = head->count;
    return(true);
    }
//  oldhead->branches.remove(head);
  for(int i=middle->branches.size();i>0;i--){
    temp = middle->branches.pop();
    if ((long)temp != (long)head){
      middle->branches.push_back(temp);
      }
    }
  removedead(oldhead);
  if(allnodes.size()<MIN_SIZE){
    makeundead();
    }
  else{
    recovermem();
    }     
//  logfile << "Moved!" << endl;
  return(false);
  }

void drrt::removedead(treenode * tn){
cout << "removedead" << endl;
  treenode * temp;
  int count=0;
  for(int i=allnodes.size();i>0;i--){
    temp = allnodes.pop();
    if ((long)temp != (long)tn){
      allnodes.push_back(temp);
      }
    else{
      count++;
      }
    }    
//  allnodes.remove(tn);
  deadnodes.push(tn);
  forall(temp, tn->branches){
    removedead(temp);
    }
  }

void drrt::makeundead(){
cout << "makeundead" << endl;
  while(deadnodes.size() != 0){
    addnode();
    }
//  logfile << "Added " << i << " new nodes!" << endl;
  }

void drrt::recovermem(){
cout << "recovermem" << endl;
  while(deadnodes.size()!=0){
    delete deadnodes.pop();
    }
  }