/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2010-2012 University of Bonn. All rights reserved.
 * Copyright (C)  2013 Frederik Heber. All rights reserved.
 * 
 *
 *   This file is part of MoleCuilder.
 *
 *    MoleCuilder is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 2 of the License, or
 *    (at your option) any later version.
 *
 *    MoleCuilder is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 */

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

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "CodePatterns/MemDebug.hpp"

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

#include "Helpers/defs.hpp"

#include "CodePatterns/Assert.hpp"
#include "LinearAlgebra/Vector.hpp"
#include "LinearAlgebra/RealSpaceMatrix.hpp"
#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/Plane.hpp"
#include "LinearAlgebra/LineSegment.hpp"
#include "LinearAlgebra/LineSegmentSet.hpp"

#include <cmath>
#include <algorithm>

// CYLINDER CODE
// ----------------------------------------------------------------------------
bool Cylinder_impl::isInside(const Vector &point) const {
  return (Vector(point[0], point[1], 0.0).NormSquared() < 1.0+MYEPSILON) &&
      (point[2] > -1.0-MYEPSILON) && (point[2] < 1.0+MYEPSILON);
}

bool Cylinder_impl::isOnSurface(const Vector &point) const {
  // on the side?
  if (fabs(Vector(point[0], point[1], 0.0).NormSquared()-1.0)<MYEPSILON &&
      (point[2] > -1.0-MYEPSILON) && (point[2] < 1.0+MYEPSILON))
    return true;
  // on top/bottom?
  if ((Vector(point[0], point[1], 0.0).NormSquared()< 1.0 + MYEPSILON) &&
      ((fabs(point[2]-1)<MYEPSILON) || (fabs(point[2]+1)<MYEPSILON)))
      return true;
  return false;

}

Vector Cylinder_impl::getNormal(const Vector &point) const throw(NotOnSurfaceException) {
  if(!isOnSurface(point)){
    throw NotOnSurfaceException() << ShapeVector(&point);
  }

  Vector n = Vector(0, 0, 0);
  if ((fabs(point[2]-1)<MYEPSILON) || (fabs(point[2]+1)<MYEPSILON))
      n += Vector(0.0, 0.0, point[2]);
  else
    n += Vector(point[0], point[1], 0.0);
  n.Normalize();
  return n;
}

Vector Cylinder_impl::getCenter() const
{
  return Vector(0.0, 0.0, 0.0);
}

double Cylinder_impl::getRadius() const
{
    return 1.0;
}

double Cylinder_impl::getVolume() const
{
	return M_PI*2.0; // pi r^2 h
}

double Cylinder_impl::getSurfaceArea() const
{
	return 2.0*M_PI*2.0; // 2 pi r h
}

LineSegmentSet Cylinder_impl::getLineIntersections(const Line &line) const {
    const Vector origin = line.getOrigin();
    const Vector direction = line.getDirection();
    
    const Vector e(direction[0], direction[1], 0.0);
    const Vector f(origin[0], origin[1], 0.0);
    const double A = e.ScalarProduct(e);
    const double B = 2.0*e.ScalarProduct(f);
    const double C = f.ScalarProduct(f) - 1.0;

    std::vector<double> solutions;

    // Common routine to solve quadratic equations, anywhere?
    const double neg_p_half = -B/(2.0*A);
    const double q = C/A;
    const double radicant = neg_p_half*neg_p_half-q;

    if (radicant > 0.0) {
        const double root = sqrt(radicant);
        solutions.push_back(neg_p_half+root);
        const double sln2 = neg_p_half-root;
        if (sln2 != solutions.back())
            solutions.push_back(sln2);
    }

    // Now get parameter for intersection with z-Planes.
    const double origin_z = origin[2];
    const double dir_z = direction[2];

    if (dir_z != 0.0) {
        solutions.push_back((-1.0-origin_z)/dir_z);
        solutions.push_back((1.0-origin_z)/dir_z);
    }

    // Calculate actual vectors from obtained parameters and check,
    // if they are actual intersections.
    std::vector<Vector> intersections;

    for(unsigned int i=0; i<solutions.size(); i++) {
        const Vector check_me(origin + direction*solutions[i]);
        if (isOnSurface(check_me))
            intersections.push_back(check_me);
    }

    LineSegmentSet result(line);
    if (intersections.size()==2)
        result.insert(LineSegment(intersections[0], intersections[1]));
    return result;
}

std::string Cylinder_impl::toString() const
{
  return "Cylinder()";
}

enum ShapeType Cylinder_impl::getType() const
{
	return CylinderType;
}

std::vector<Vector> Cylinder_impl::getHomogeneousPointsOnSurface(const size_t N) const {
    const double nz_float = sqrt(N/M_PI);
    const int nu = round(N/nz_float);
    const int nz = round(nz_float);

    const double dphi = 2.0*M_PI/nu;
    const double dz = 2.0/nz;

    std::vector<Vector> result;
    
    for(int useg=0; useg<nu; useg++)
        for(int zseg=0; zseg<=nz; zseg++)
            result.push_back(Vector(cos(useg*dphi), sin(useg*dphi), zseg*dz-1.0));

    return result;
}

std::vector<Vector> Cylinder_impl::getHomogeneousPointsInVolume(const size_t N) const {
    const double nz_float = pow(N/(2.0*M_PI), 1.0/3.0);
    const int nu = round(nz_float*M_PI);
    const int nr = round(nz_float*0.5);
    const int nz = round(nz_float);
    
    const double dphi = 2.0*M_PI/nu;
    const double dz = 2.0/nz;
    const double dr = 1.0/nr;

    std::vector<Vector> result;
    
    for(int useg=0; useg<nu; useg++)
        for(int zseg=0; zseg<nz; zseg++)
            for(int rseg=0; rseg<nr; rseg++)
            {
                const double r = dr+rseg*dr;
                result.push_back(Vector(r*cos(useg*dphi), r*sin(useg*dphi), zseg*dz-1.0));
            }

    return result;
}

Shape Cylinder() {
  Shape::impl_ptr impl = Shape::impl_ptr(new Cylinder_impl());
  return Shape(impl);
}

Shape Cylinder(const Vector &center, const double xrot, const double yrot,
        const double height, const double radius)
{
    RealSpaceMatrix rot;
    rot.setRotation(xrot, yrot, 0.0);

    return translate(
                transform(
                    stretch(
                        Cylinder(),
                    Vector(radius, radius, height*0.5)),
                rot),
            center);
}
// ----------------------------------------------------------------------------

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

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

Vector Sphere_impl::getNormal(const Vector &point) const throw(NotOnSurfaceException){
  if(!isOnSurface(point)){
    throw NotOnSurfaceException() << ShapeVector(&point);
  }
  return point;
}

Vector Sphere_impl::getCenter() const
{
  return Vector(0.,0.,0.);
}

double Sphere_impl::getRadius() const
{
  return 1.;
}

double Sphere_impl::getVolume() const
{
	return (4./3.)*M_PI; // 4/3 pi r^3
}

double Sphere_impl::getSurfaceArea() const
{
	return 2.*M_PI;	// 2 pi r^2
}


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

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

enum ShapeType Sphere_impl::getType() const
{
	return SphereType;
}

/**
 * algorithm taken from http://www.cgafaq.info/wiki/Evenly_distributed_points_on_sphere
 * \param N number of points on surface
 */
std::vector<Vector> Sphere_impl::getHomogeneousPointsOnSurface(const size_t N) const
{
  std::vector<Vector> PointsOnSurface;
  if (true) {
    // Exactly N points but not symmetric.

    // This formula is derived by finding a curve on the sphere that spirals down from
    // the north pole to the south pole keeping a constant distance between consecutive turns.
    // The curve is then parametrized by arch length and evaluated in constant intervals.
    double a = sqrt(N) * 2;
    for (size_t i=0; i<N; ++i){
      double t0 = ((double)i + 0.5) / (double)N;
      double t = (sqrt(t0) - sqrt(1.0 - t0) + 1.0) / 2.0 * M_PI;
      Vector point;
      point.Zero();
      point[0] = sin(t) * sin(t * a);
      point[1] = sin(t) * cos(t * a);
      point[2] = cos(t);
      PointsOnSurface.push_back(point);
    }
    ASSERT(PointsOnSurface.size() == N,
        "Sphere_impl::getHomogeneousPointsOnSurface() did not create "
        +::toString(N)+" but "+::toString(PointsOnSurface.size())+" points.");
  } else {
    // Symmetric but only approximately N points.
    double a=4*M_PI/N;
    double d= sqrt(a);
    size_t Mtheta=round(M_PI/d);
    double dtheta=M_PI/Mtheta;
    double dphi=a/dtheta;
    for (size_t m=0; m<Mtheta; ++m)
    {
      double theta=M_PI*(m+0.5)/Mtheta;
      size_t Mphi=round(2*M_PI*sin(theta)/dphi);
      for (size_t n=0; n<Mphi;++n)
      {
        double phi= 2*M_PI*n/Mphi;
        Vector point;
        point.Zero();
        point[0]=sin(theta)*cos(phi);
        point[1]=sin(theta)*sin(phi);
        point[2]=cos(theta);
        PointsOnSurface.push_back(point);
      }
    }
  }
  return PointsOnSurface;
}

std::vector<Vector> Sphere_impl::getHomogeneousPointsInVolume(const size_t N) const {
	ASSERT(0,
			"Sphere_impl::getHomogeneousPointsInVolume() - not implemented.");
	return std::vector<Vector>();
}

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) const{
  return (point[0]>=-MYEPSILON && point[0]<=1+MYEPSILON) && (point[1]>=-MYEPSILON && point[1]<=1+MYEPSILON) && (point[2]>=-MYEPSILON && point[2]<=1+MYEPSILON);
}

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

Vector Cuboid_impl::getNormal(const Vector &point) const throw(NotOnSurfaceException){
  if(!isOnSurface(point)){
    throw NotOnSurfaceException() << ShapeVector(&point);
  }
  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(point[i])<MYEPSILON) || (fabs(point[i]-1.)<MYEPSILON)){
      // add the scaled (-1/+1) Vector to the set of surface vectors
      res[i] = point[i] * 2.0 - 1.0;
    }
  }
  ASSERT((fabs(res.NormSquared() - 1.) >= -MYEPSILON)
      && (fabs(res.NormSquared() - 3.) >= -MYEPSILON),
      "To many or to few sides found for this Vector");

  res.Normalize();
  return res;
}


Vector Cuboid_impl::getCenter() const
{
  return Vector(0.5,0.5,0.5);
}

double Cuboid_impl::getRadius() const
{
  return .5;
}

double Cuboid_impl::getVolume() const
{
	return 1.; // l^3
}

double Cuboid_impl::getSurfaceArea() const
{
	return 6.;	// 6 * l^2
}

LineSegmentSet Cuboid_impl::getLineIntersections(const Line &line) const{
  LineSegmentSet res(line);
  // get the intersection on each of the six faces
  std::vector<Vector> intersections;
  intersections.resize(2);
  int c=0;
  int x[2]={-1,+1};
  for(int i=NDIM;i--;){
    for(int j=0;j<2;++j){
      if(c==2) goto end; // I know this sucks, but breaking two loops is stupid
      Vector base;
      base[i]=x[j];
      // 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;
}

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

enum ShapeType Cuboid_impl::getType() const
{
	return CuboidType;
}

/**
 * \param N number of points on surface
 */
std::vector<Vector> Cuboid_impl::getHomogeneousPointsOnSurface(const size_t N) const {
  std::vector<Vector> PointsOnSurface;
  // sides
  int n = sqrt((N - 1) / 6) + 1;
  for (int i=0; i<=n; i++){
    double ii = (double)i / (double)n;
    for (int k=0; k<n; k++){
      double kk = (double)k / (double)n;
      PointsOnSurface.push_back(Vector(ii, kk, 1));
      PointsOnSurface.push_back(Vector(ii, 1, 1-kk));
      PointsOnSurface.push_back(Vector(ii, 1-kk, 0));
      PointsOnSurface.push_back(Vector(ii, 0, kk));
    }
  }
  // top and bottom
  for (int i=1; i<n; i++){
    double ii = (double)i / (double)n;
    for (int k=1; k<n; k++){
      double kk = (double)k / (double)n;
      PointsOnSurface.push_back(Vector(0, ii, kk));
      PointsOnSurface.push_back(Vector(1, ii, kk));
    }
  }
  return PointsOnSurface;
}

std::vector<Vector> Cuboid_impl::getHomogeneousPointsInVolume(const size_t N) const {
	ASSERT(0,
			"Cuboid_impl::getHomogeneousPointsInVolume() - not implemented.");
	return std::vector<Vector>();
}

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] = std::min(corner1[i],corner2[i]);
    sortedC2[i] = std::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);
}