/*
 * BaseShapes_impl.cpp
 *
 *  Created on: Jun 18, 2010
 *      Author: crueger
 */

#include "Shapes/BaseShapes.hpp"
#include "Shapes/BaseShapes_impl.hpp"
#include "Shapes/ShapeOps.hpp"

#include "vector.hpp"
#include "Helpers/Assert.hpp"

#include "Line.hpp"
#include "Plane.hpp"
#include "LineSegment.hpp"
#include "LineSegmentSet.hpp"

#include <cmath>
#include <algorithm>

bool Sphere_impl::isInside(const Vector &point){
  return point.NormSquared()<=1;
}

bool Sphere_impl::isOnSurface(const Vector &point){
  return fabs(point.NormSquared()-1)<MYEPSILON;
}

Vector Sphere_impl::getNormal(const Vector &point) throw(NotOnSurfaceException){
  if(!isOnSurface(point)){
    throw NotOnSurfaceException(__FILE__,__LINE__);
  }
  return point;
}

LineSegmentSet Sphere_impl::getLineIntersections(const Line &line){
  LineSegmentSet res(line);
  std::vector<Vector> intersections = line.getSphereIntersections();
  if(intersections.size()==2){
    res.insert(LineSegment(intersections[0],intersections[1]));
  }
  return res;
}

string Sphere_impl::toString(){
  return "Sphere()";
}

Shape Sphere(){
  Shape::impl_ptr impl = Shape::impl_ptr(new Sphere_impl());
  return Shape(impl);
}

Shape Sphere(const Vector &center,double radius){
  return translate(resize(Sphere(),radius),center);
}

Shape Ellipsoid(const Vector &center, const Vector &radius){
  return translate(stretch(Sphere(),radius),center);
}

bool Cuboid_impl::isInside(const Vector &point){
  return fabs(point[0])<=1 && fabs(point[1])<=1 && fabs(point[2])<=1;
}

bool Cuboid_impl::isOnSurface(const Vector &point){
  bool retVal = isInside(point);
  // test all borders of the cuboid
  // double fabs
  retVal = retVal &&
           ((fabs(fabs(point[0])-1)  < MYEPSILON) ||
            (fabs(fabs(point[1])-1)  < MYEPSILON) ||
            (fabs(fabs(point[2])-1)  < MYEPSILON));
  return retVal;
}

Vector Cuboid_impl::getNormal(const Vector &point) throw(NotOnSurfaceException){
  if(!isOnSurface(point)){
    throw NotOnSurfaceException(__FILE__,__LINE__);
  }
  Vector res;
  // figure out on which sides the Vector lies (maximum 3, when it is in a corner)
  for(int i=NDIM;i--;){
    if(fabs(fabs(point[i])-1)<MYEPSILON){
      // add the scaled (-1/+1) Vector to the set of surface vectors
      res[i] = point[i];
    }
  }
  ASSERT(res.NormSquared()>=1 && res.NormSquared()<=3,"To many or to few sides found for this Vector");

  res.Normalize();
  return res;
}

LineSegmentSet Cuboid_impl::getLineIntersections(const Line &line){
  LineSegmentSet res(line);
  // get the intersection on each of the six faces
  vector<Vector> intersections;
  intersections.resize(2);
  int c=0;
  int x[2]={-1,+1};
  for(int i=NDIM;i--;){
    for(int p=0;p<2;++p){
      if(c==2) goto end; // I know this sucks, but breaking two loops is stupid
      Vector base;
      base[i]=x[p];
      // base now points to the surface and is normal to it at the same time
      Plane p(base,base);
      Vector intersection = p.GetIntersection(line);
      if(isInside(intersection)){
        // if we have a point on the edge it might already be contained
        if(c==1 && intersections[0]==intersection)
          continue;
        intersections[c++]=intersection;
      }
    }
  }
  end:
  if(c==2){
    res.insert(LineSegment(intersections[0],intersections[1]));
  }
  return res;
}

string Cuboid_impl::toString(){
  return "Cuboid()";
}

Shape Cuboid(){
  Shape::impl_ptr impl = Shape::impl_ptr(new Cuboid_impl());
  return Shape(impl);
}

Shape Cuboid(const Vector &corner1, const Vector &corner2){
  // make sure the two edges are upper left front and lower right back
  Vector sortedC1;
  Vector sortedC2;
  for(int i=NDIM;i--;){
    sortedC1[i] = min(corner1[i],corner2[i]);
    sortedC2[i] = max(corner1[i],corner2[i]);
    ASSERT(corner1[i]!=corner2[i],"Given points for cuboid edges did not define a valid space");
  }
  // get the middle point
  Vector middle = (1./2.)*(sortedC1+sortedC2);
  Vector factors = sortedC2-middle;
  return translate(stretch(Cuboid(),factors),middle);
}