#include "BSPTree.h"
#include <iostream>

BSPTree::BSPTree(const std::list<Triangle> &triangles) : frontTree(0), backTree(0) {
  if(triangles.empty())
    return;

  Vertex center = findCenter(triangles);
  std::cout << "Center at (" << center.getX() << ", " << center.getY() << ", " << center.getZ() << ")" << std::endl;

  const Triangle *planeT = findNearestTriangle(triangles, center);

  plane = Plane(*planeT);
  std::cout << "The plane normal is (" << plane.getNormal().getX() << ", " << plane.getNormal().getY() << ", " << plane.getNormal().getZ() << ")" << std::endl;

  std::list<Triangle> front, back;

  for(std::list<Triangle>::const_iterator t = triangles.begin(); t != triangles.end(); ++t) {
    if(plane.contains(*t)) {
      this->triangles.push_back(*t);
      continue;
    }
    else if(plane.isInFront(*t)) {
      front.push_back(*t);
      continue;
    }
    else if(plane.isBehind(*t)) {
      back.push_back(*t);
      continue;
    }
  }

  std::cout << "All in all: " << triangles.size() << " triangles" << std::endl;
  std::cout << "Coplanar: " << this->triangles.size() << " triangles" << std::endl;
  std::cout << "Front: " << front.size() << " triangles" << std::endl;
  std::cout << "Back: " << back.size() << " triangles" << std::endl;

  if(!front.empty())
    frontTree = new BSPTree(front);

  if(!back.empty())
    backTree = new BSPTree(back);
}


Vertex BSPTree::findCenter(const std::list<Triangle> &triangles) {
  Vertex v;

  for(std::list<Triangle>::const_iterator t = triangles.begin(); t != triangles.end(); ++t) {
    v += t->getCenter();
  }

  return v/triangles.size();
}

const Triangle* BSPTree::findNearestTriangle(const std::list<Triangle> &triangles, const Vertex &v) {
  if(triangles.empty())
    return 0;

  std::list<Triangle>::const_iterator t = triangles.begin();

  const Triangle *current = &*t;
  float distanceSq = current->getCenter().distanceSq(v);

  for(++t; t != triangles.end(); ++t) {
    float d = t->getCenter().distanceSq(v);

    if(d < distanceSq) {
      current = &*t;
      distanceSq = d;
    }
  }

  Vertex tCenter = current->getCenter();
  std::cout << "Nearest triangle center at (" << tCenter.getX() << ", " << tCenter.getY() << ", " << tCenter.getZ() << ")" << std::endl;
  std::cout << "DistanceSq is " << distanceSq << std::endl;

  return current;
}