#include "BSPTree.h"

#include <iostream>

vmml::vec3f BSPTree::Plane::intersection(const vmml::vec3f &p, const vmml::vec3f &dir) const {
  float r = (d - p.dot(normal))/dir.dot(normal);

  return p + r*dir;
}

void BSPTree::Plane::partition(const Triangle &t, std::list<Triangle> *front, std::list<Triangle> *back) const {
  for(int i = 0; i < 3; ++i) {
    if(contains(t.getVertex(i))) {
      const vmml::vec3f *v[3] = {&t.getVertex(i), &t.getVertex((i+1)%3), &t.getVertex((i+2)%3)};

      vmml::vec3f is = intersection(*v[1], *v[2]-*v[1]);

      if(isInFront(*v[1])) {
        front->push_back(Triangle(*v[0], *v[1], is, t.getColor()));
        back->push_back(Triangle(*v[0], is, *v[2], t.getColor()));
      }
      else {
        back->push_back(Triangle(*v[0], *v[1], is, t.getColor()));
        front->push_back(Triangle(*v[0], is, *v[2], t.getColor()));
      }

      return;
    }
  }

  for(int i = 0; i < 3; ++i) {
    const vmml::vec3f *v[3] = {&t.getVertex(i), &t.getVertex((i+1)%3), &t.getVertex((i+2)%3)};

    if((isInFront(*v[0]) && isBehind(*v[1]) && isBehind(*v[2]))
        || (isBehind(*v[0]) && isInFront(*v[1]) && isInFront(*v[2]))) {
      vmml::vec3f is1 = intersection(*v[0], *v[1]-*v[0]);
      vmml::vec3f is2 = intersection(*v[0], *v[2]-*v[0]);

      if(isInFront(*v[0])) {
        front->push_back(Triangle(*v[0], is1, is2, t.getColor()));
        back->push_back(Triangle(is1, *v[1], is2, t.getColor()));
        back->push_back(Triangle(*v[1], *v[2], is2, t.getColor()));
      }
      else {
        back->push_back(Triangle(*v[0], is1, is2, t.getColor()));
        front->push_back(Triangle(is1, *v[1], is2, t.getColor()));
        front->push_back(Triangle(*v[1], *v[2], is2, t.getColor()));
      }

      return;
    }
  }
}


BSPTree::BSPTree(const std::list<Triangle> &triangles) : frontTree(0), backTree(0) {
  const Triangle *planeT = findNearestTriangle(triangles, findCenter(triangles));

  if(!planeT)
    return;

  plane = Plane(*planeT);

  std::list<Triangle> front, back;

  for(std::list<Triangle>::const_iterator t = triangles.begin(); t != triangles.end(); ++t) {
    if(t->getNormal().squared_length() == 0)
      continue;

    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::list<Triangle> frontPart, backPart;
    plane.partition(*t, &frontPart, &backPart);
    front.splice(front.end(), frontPart);
    back.splice(back.end(), backPart);
  }

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

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

BSPTree& BSPTree::operator=(const BSPTree &tree) {
  if(frontTree) {
    delete frontTree;
    frontTree = 0;
  }

  if(backTree) {
    delete backTree;
    backTree = 0;
  }

  plane = tree.plane;
  triangles = tree.triangles;

  if(tree.frontTree)
    frontTree = new BSPTree(*tree.frontTree);

  if(tree.backTree)
    backTree = new BSPTree(*tree.backTree);
  
  return *this;
}

vmml::vec3f BSPTree::findCenter(const std::list<Triangle> &triangles) {
  vmml::vec3f 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 vmml::vec3f &v) {
  const Triangle *current = 0;
  float distanceSq;

  for(std::list<Triangle>::const_iterator t = triangles.begin(); t != triangles.end(); ++t) {
    if(t->getNormal().squared_length() == 0)
      continue;

    float d = t->getCenter().squared_distance(v);

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

  return current;
}