/**
 * Stone of Orthanc
 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
 * Department, University Hospital of Liege, Belgium
 * Copyright (C) 2017-2019 Osimis S.A., Belgium
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * This program 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
 * Affero General Public License for more details.
 * 
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 **/


#include "GeometryToolbox.h"

#include <Core/Logging.h>
#include <Core/OrthancException.h>

#include <cassert>

namespace OrthancStone
{
  namespace GeometryToolbox
  {
    void ProjectPointOntoPlane(Vector& result,
                               const Vector& point,
                               const Vector& planeNormal,
                               const Vector& planeOrigin)
    {
      double norm =  boost::numeric::ublas::norm_2(planeNormal);
      if (LinearAlgebra::IsCloseToZero(norm))
      {
        // Division by zero
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      // Make sure the norm of the normal is 1
      Vector n;
      n = planeNormal / norm;

      // Algebraic form of line–plane intersection, where the line passes
      // through "point" along the direction "normal" (thus, l == n)
      // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form
      result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point;
    }

    /*
    undefined results if vector are not 3D
    */
    void ProjectPointOntoPlane2(
      double& resultX,
      double& resultY,
      double& resultZ,
      const Vector& point,
      const Vector& planeNormal,
      const Vector& planeOrigin)
    {
      double pointX = point[0];
      double pointY = point[1];
      double pointZ = point[2];

      double planeNormalX = planeNormal[0];
      double planeNormalY = planeNormal[1];
      double planeNormalZ = planeNormal[2];

      double planeOriginX = planeOrigin[0];
      double planeOriginY = planeOrigin[1];
      double planeOriginZ = planeOrigin[2];

      double normSq = (planeNormalX * planeNormalX) + (planeNormalY * planeNormalY) + (planeNormalZ * planeNormalZ);

      // Algebraic form of line–plane intersection, where the line passes
      // through "point" along the direction "normal" (thus, l == n)
      // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form

      if (LinearAlgebra::IsNear(1.0, normSq))
      {
        double nX = planeNormalX;
        double nY = planeNormalY;
        double nZ = planeNormalZ;
        
        double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ;

        resultX = prod * nX + pointX; 
        resultY = prod * nY + pointY; 
        resultZ = prod * nZ + pointZ;
      }
      else
      {
        double norm = sqrt(normSq);
        if (LinearAlgebra::IsCloseToZero(norm))
        {
          // Division by zero
          throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
        }
        double invNorm = 1.0 / norm;
        double nX = planeNormalX * invNorm;
        double nY = planeNormalY * invNorm;
        double nZ = planeNormalZ * invNorm;

        double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ;

        resultX = prod * nX + pointX;
        resultY = prod * nY + pointY;
        resultZ = prod * nZ + pointZ;
      }
    }

    bool IsParallelOrOpposite(bool& isOpposite,
                              const Vector& u,
                              const Vector& v)
    {
      // The dot product of the two vectors gives the cosine of the angle
      // between the vectors
      // https://en.wikipedia.org/wiki/Dot_product

      double normU = boost::numeric::ublas::norm_2(u);
      double normV = boost::numeric::ublas::norm_2(v);

      if (LinearAlgebra::IsCloseToZero(normU) ||
          LinearAlgebra::IsCloseToZero(normV))
      {
        return false;
      }

      double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV);

      // The angle must be zero, so the cosine must be almost equal to
      // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true)

      if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0))
      {
        isOpposite = false;
        return true;
      }
      else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0))
      {
        isOpposite = true;
        return true;
      }
      else
      {
        return false;
      }
    }


    bool IsParallel(const Vector& u,
                    const Vector& v)
    {
      bool isOpposite;
      return (IsParallelOrOpposite(isOpposite, u, v) &&
              !isOpposite);
    }


    bool IntersectTwoPlanes(Vector& p,
                            Vector& direction,
                            const Vector& origin1,
                            const Vector& normal1,
                            const Vector& origin2,
                            const Vector& normal2)
    {
      // This is "Intersection of 2 Planes", possibility "(C) 3 Plane
      // Intersect Point" of:
      // http://geomalgorithms.com/a05-_intersect-1.html

      // The direction of the line of intersection is orthogonal to the
      // normal of both planes
      LinearAlgebra::CrossProduct(direction, normal1, normal2);

      double norm = boost::numeric::ublas::norm_2(direction);
      if (LinearAlgebra::IsCloseToZero(norm))
      {
        // The two planes are parallel or coincident
        return false;
      }

      double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1);
      double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2);
      Vector tmp = d2 * normal1 - d1 * normal2;

      LinearAlgebra::CrossProduct(p, tmp, direction);
      p /= norm;

      return true;
    }


    bool ClipLineToRectangle(double& x1,  // Coordinates of the clipped line (out)
                             double& y1,
                             double& x2,
                             double& y2,
                             const double ax,  // Two points defining the line (in)
                             const double ay,
                             const double bx,
                             const double by,
                             const double& xmin,   // Coordinates of the rectangle (in)
                             const double& ymin,
                             const double& xmax,
                             const double& ymax)
    {
      // This is Skala algorithm for rectangles, "A new approach to line
      // and line segment clipping in homogeneous coordinates"
      // (2005). This is a direct, non-optimized translation of Algorithm
      // 2 in the paper.

      static const uint8_t tab1[16] = { 255 /* none */,
                                        0,
                                        0,
                                        1,
                                        1,
                                        255 /* na */,
                                        0,
                                        2,
                                        2,
                                        0,
                                        255 /* na */,
                                        1,
                                        1,
                                        0,
                                        0,
                                        255 /* none */ };


      static const uint8_t tab2[16] = { 255 /* none */,
                                        3,
                                        1,
                                        3,
                                        2,
                                        255 /* na */,
                                        2,
                                        3,
                                        3,
                                        2,
                                        255 /* na */,
                                        2,
                                        3,
                                        1,
                                        3,
                                        255 /* none */ };

      // Create the coordinates of the rectangle
      Vector x[4];
      LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0);
      LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0);
      LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0);
      LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0);

      // Move to homogoneous coordinates in 2D
      Vector p;

      {
        Vector a, b;
        LinearAlgebra::AssignVector(a, ax, ay, 1.0);
        LinearAlgebra::AssignVector(b, bx, by, 1.0);
        LinearAlgebra::CrossProduct(p, a, b);
      }

      uint8_t c = 0;

      for (unsigned int k = 0; k < 4; k++)
      {
        if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0)
        {
          c |= (1 << k);
        }
      }

      assert(c < 16);

      uint8_t i = tab1[c];
      uint8_t j = tab2[c];

      if (i == 255 || j == 255)
      {
        return false;   // No intersection
      }
      else
      {
        Vector a, b, e;
        LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]);
        LinearAlgebra::CrossProduct(a, p, e);
        LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]);
        LinearAlgebra::CrossProduct(b, p, e);

        // Go back to non-homogeneous coordinates
        x1 = a[0] / a[2];
        y1 = a[1] / a[2];
        x2 = b[0] / b[2];
        y2 = b[1] / b[2];

        return true;
      }
    }


    void GetPixelSpacing(double& spacingX, 
                         double& spacingY,
                         const Orthanc::DicomMap& dicom)
    {
      Vector v;

      if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING))
      {
        if (v.size() != 2 ||
            v[0] <= 0 ||
            v[1] <= 0)
        {
          LOG(ERROR) << "Bad value for PixelSpacing tag";
          throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat);
        }
        else
        {
          // WARNING: X/Y are swapped (Y comes first)
          spacingX = v[1];
          spacingY = v[0];
        }
      }
      else
      {
        // The "PixelSpacing" is of type 1C: It could be absent, use
        // default value in such a case
        spacingX = 1;
        spacingY = 1;
      }
    }

    
    Matrix CreateRotationMatrixAlongX(double a)
    {
      // Rotate along X axis (R_x)
      // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
      Matrix r(3, 3);
      r(0,0) = 1;
      r(0,1) = 0;
      r(0,2) = 0;
      r(1,0) = 0;
      r(1,1) = cos(a);
      r(1,2) = -sin(a);
      r(2,0) = 0;
      r(2,1) = sin(a);
      r(2,2) = cos(a);
      return r;
    }
    

    Matrix CreateRotationMatrixAlongY(double a)
    {
      // Rotate along Y axis (R_y)
      // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
      Matrix r(3, 3);
      r(0,0) = cos(a);
      r(0,1) = 0;
      r(0,2) = sin(a);
      r(1,0) = 0;
      r(1,1) = 1;
      r(1,2) = 0;
      r(2,0) = -sin(a);
      r(2,1) = 0;
      r(2,2) = cos(a);
      return r;
    }


    Matrix CreateRotationMatrixAlongZ(double a)
    {
      // Rotate along Z axis (R_z)
      // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
      Matrix r(3, 3);
      r(0,0) = cos(a);
      r(0,1) = -sin(a);
      r(0,2) = 0;
      r(1,0) = sin(a);
      r(1,1) = cos(a);
      r(1,2) = 0;
      r(2,0) = 0;
      r(2,1) = 0;
      r(2,2) = 1;
      return r;
    }    


    Matrix CreateTranslationMatrix(double dx,
                                   double dy,
                                   double dz)
    {
      Matrix m = LinearAlgebra::IdentityMatrix(4);
      m(0,3) = dx;
      m(1,3) = dy;
      m(2,3) = dz;
      return m;    
    }


    Matrix CreateScalingMatrix(double sx,
                               double sy,
                               double sz)
    {
      Matrix m = LinearAlgebra::IdentityMatrix(4);
      m(0,0) = sx;
      m(1,1) = sy;
      m(2,2) = sz;
      return m;    
    }


    bool IntersectPlaneAndSegment(Vector& p,
                                  const Vector& normal,
                                  double d,
                                  const Vector& edgeFrom,
                                  const Vector& edgeTo)
    {
      // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection

      // Check for parallel line and plane
      Vector direction = edgeTo - edgeFrom;
      double denominator = boost::numeric::ublas::inner_prod(direction, normal);

      if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
      {
        return false;
      }
      else
      {
        // Compute intersection
        double t = -(normal[0] * edgeFrom[0] + 
                     normal[1] * edgeFrom[1] + 
                     normal[2] * edgeFrom[2] + d) / denominator;

        if (t >= 0 && t <= 1)
        {
          // The intersection lies inside edge segment
          p = edgeFrom + t * direction;
          return true;
        }
        else
        {
          return false;
        }
      }
    }


    bool IntersectPlaneAndLine(Vector& p,
                               const Vector& normal,
                               double d,
                               const Vector& origin,
                               const Vector& direction)
    {
      // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection

      // Check for parallel line and plane
      double denominator = boost::numeric::ublas::inner_prod(direction, normal);

      if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
      {
        return false;
      }
      else
      {
        // Compute intersection
        double t = -(normal[0] * origin[0] + 
                     normal[1] * origin[1] + 
                     normal[2] * origin[2] + d) / denominator;

        p = origin + t * direction;
        return true;
      }
    }


    void AlignVectorsWithRotation(Matrix& r,
                                  const Vector& a,
                                  const Vector& b)
    {
      // This is Rodrigues' rotation formula:
      // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation

      // Check also result A4.6 from "Multiple View Geometry in Computer
      // Vision - 2nd edition" (p. 584)
  
      if (a.size() != 3 ||
          b.size() != 3)
      {
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
      }

      double aNorm = boost::numeric::ublas::norm_2(a);
      double bNorm = boost::numeric::ublas::norm_2(b);

      if (LinearAlgebra::IsCloseToZero(aNorm) ||
          LinearAlgebra::IsCloseToZero(bNorm))
      {
        LOG(ERROR) << "Vector with zero norm";
        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);      
      }

      Vector aUnit, bUnit;
      aUnit = a / aNorm;
      bUnit = b / bNorm;

      Vector v;
      LinearAlgebra::CrossProduct(v, aUnit, bUnit);

      double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);

      if (LinearAlgebra::IsCloseToZero(1 + cosine))
      {
        // "a == -b": TODO
        throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
      }

      Matrix k;
      LinearAlgebra::CreateSkewSymmetric(k, v);

#if 0
      double sine = boost::numeric::ublas::norm_2(v);

      r = (boost::numeric::ublas::identity_matrix<double>(3) +
           sine * k + 
           (1 - cosine) * boost::numeric::ublas::prod(k, k));
#else
      r = (boost::numeric::ublas::identity_matrix<double>(3) +
           k + 
           boost::numeric::ublas::prod(k, k) / (1 + cosine));
#endif
    }
  }
}