/**
**
** @RDE_HEADER@
** Rasterize functional path geometries to polylines
** $Id$
** $HeadURL$
** $Author$
**
** -----------------------------------------------------------------------------------------------
**
** Hardware-Environment: Intel-PC
** Software-Environment: Windows 7, Qt 5.4. ArcGIS 10.2.5
**
** Language ........: C++ 11
**
** -----------------------------------------------------------------------------------------------
**
** Author ...........: Jörg Turla, SPD G, Tel. 3132
**
** -----------------------------------------------------------------------------------------------
**
** Notes .........:
**
**   12.08.2015, JT  : First edition for use with Qt5
**
** -----------------------------------------------------------------------------------------------
*/

#include "curve.h"
#include "path_rasterer.h"


#include <Common/exception.h>

namespace SituationMap { namespace MathUtils
{

   static QPointF calcBezierVertex( double t, const QPointF control[4] )
   {
      double u, u2, u3, t2, t3;
      double xx, yy;

      t2 = t*t;
      t3 = t2*t;
      u  = 1.0 - t;
      u2 = u*u;
      u3 = u2*u;
      xx = control[0].x()*u3 + 3.0*(control[1].x()*t*u2 + control[2].x()*t2*u) + control[3].x()*t3;
      yy = control[0].y()*u3 + 3.0*(control[1].y()*t*u2 + control[2].y()*t2*u) + control[3].y()*t3;

      return QPointF(xx,yy);

   } /* calcBezierVertex */


   static QPointF calcBezierTangent( double t, const QPointF control[4] )
   {
      double t2(t*t);
      double u(1.0-t);
      double u2(u*u);
      double xx(3.0*((control[1].x()-control[0].x())*u2 + (control[2].x()-control[1].x())*t*u*2.0 + (control[3].x()-control[2].x())*t2));
      double yy(3.0*((control[1].y()-control[0].y())*u2 + (control[2].y()-control[1].y())*t*u*2.0 + (control[3].y()-control[2].y())*t2));

      return QPointF(xx,yy);

   } /* calcBezierTangent */


   static double hypotBezierTangent( double t, const QPointF control[4] )
   {
      QPointF p(calcBezierTangent(t,control));
      return hypot(p.x(),p.y());
   } /* hypotBezierTangent */


   static double simpsonApproximation( const QPointF control[4], double a, double b )
   {
      const unsigned int NLenSteps = 800;

      double t( b-a );
      double h( t/NLenSteps );

      double sum( 0.0 );

      // Zusammengesetzte Integration nach der Simpson-Regel

      double s1( 0.0 );
      double s2( 0.0 );

      for (unsigned int i = 0; i < NLenSteps; i++)
      {
         s1 += hypotBezierTangent( a+h*i, control );
         s2 += hypotBezierTangent( a+h*(double(i)-0.5), control );
      }
      s2 += hypotBezierTangent( a+h*(double(NLenSteps)-0.5), control );

      sum += h/6.0 *( hypotBezierTangent(a,control)+hypotBezierTangent(b,control)+2.0*s1+4.0*s2);

      return sum;

   } /* simpsonApproximation */


   static inline void buildControlPoints( const QPointF &  p_i,
                                          const QPointF &  p_i1,
                                          const QPointF &  p_i2,
                                          QPointF          control[4],
                                          bool             startSegm  = false,
                                          bool             endSegm    = false  )
   {
      if (startSegm)
      {
         control[0] = p_i;
         control[1] = p_i*0.333 + p_i1*0.667;
      }
      else
      {
         control[0] = p_i*0.5   + p_i1*0.5;
         control[1] = p_i*0.167 + p_i1*0.833;
      }

      if (endSegm)
      {
         control[2] = p_i1*0.667 + p_i2*0.333;
         control[3] = p_i2;
      }
      else
      {
         control[2] = p_i1*0.833 + p_i2*0.167;
         control[3] = p_i1*0.5   + p_i2*0.5;
      }
   } /* buildControlPoints */



   SmoothLine::SmoothLine(const QVector<QPointF> & vertexVector, bool closedCurve )
   {
      SmoothLine::vertexVector( vertexVector, closedCurve );
   } /* BezierSpline::SmoothLine */

   void SmoothLine::vertexVector( const QVector<QPointF> & vertexVector, bool closedCurve )
   {
      _closedCurve = closedCurve;
      _vertexVector = vertexVector;
   } /*  SmoothLine::vertexVector */


   qreal SmoothLine::maximumCurveParameter() const
   {
      if (_vertexVector.size() > 2)
      {
         return (_closedCurve) ? (qreal)_vertexVector.size() : (qreal)_vertexVector.size() - 2.0;
      }
      return 0;
   } /* SmoothLine::maximumCurveParameter */


   QPointF SmoothLine::positionOnCurve( qreal t ) const
   {
      if (_vertexVector.size() < 3)
      {
         if (_vertexVector.size() > 1)
         {
            // It's just a straight line
            if (t < 0.0) t = 1.0-t;

            if ((t >= 0.0) && (t <= 1.0))
            {
               const QPointF & v0 = _vertexVector[0];
               const QPointF & v1 = _vertexVector[1];
               return v0 + (v1-v0)*t;
            }
         }
      }
      else
      {
         int  maxSegm = _vertexVector.size() - ((_closedCurve) ? 1 : 3);

         if (t < 0.0) t = double(maxSegm+1) - t;

         int segment( floor(t) );

         if ((segment >= 0) && (segment <= maxSegm))
         {
            QPointF  control[4];
            QPointF  p[3];

            if ((_closedCurve) && (segment == (maxSegm-1)))
            {
               p[0] = _vertexVector[_vertexVector.size()-2];
               p[1] = _vertexVector.back();
               p[2] = _vertexVector[0];

               buildControlPoints( p[0], p[1], p[2], control, false, false );
            }
            else if ((_closedCurve) && (segment == maxSegm))
            {
               p[0] = _vertexVector.back();
               p[1] = _vertexVector[0];
               p[2] = _vertexVector[1];

               buildControlPoints( p[0], p[1], p[2], control, false, false );
            }
            else
            {
               p[0] = _vertexVector[segment  ];
               p[1] = _vertexVector[segment+1];
               p[2] = _vertexVector[segment+2];

               buildControlPoints( p[0], p[1], p[2], control, ((segment == 0) && (!_closedCurve)), (segment == maxSegm) );
            }
            return calcBezierVertex( t-double(segment), control );
         }
         else
         {
            if (t == (maxSegm+1))
            {
               QPointF  control[4];
               QPointF  p[3];

               if (_closedCurve)
               {
                  p[0] = _vertexVector[0];
                  p[1] = _vertexVector[1];
                  p[2] = _vertexVector[2];

                  buildControlPoints( p[0], p[1], p[2], control, true, false );
                  return calcBezierVertex( 0.0, control );
               }
               else
               {
                  p[0] = _vertexVector[maxSegm  ];
                  p[1] = _vertexVector[maxSegm+1];
                  p[2] = _vertexVector[maxSegm+2];

                  buildControlPoints( p[0], p[1], p[2], control, (maxSegm == 0), true );
                  return calcBezierVertex( 1.0, control );
               }
            }
         }
      }
      return QPointF(0.0,0.0);
   } /* SmoothLine::positionOnCurve */


   QPointF SmoothLine::directionOnCurve( qreal t ) const
   {
      if (_vertexVector.size() < 3)
      {
         if (_vertexVector.size() >1)
         {
            // It's just a straight line
            if (t < 0.0) t = 1.0-t;

            if ((t >= 0.0) && (t <= 1.0))
            {
               return (_vertexVector[1]-_vertexVector[0]);
            }
         }
      }
      else
      {
         int  maxSegm = _vertexVector.size() - ((_closedCurve) ? 1 : 3);

         if (t < 0.0) t = double(maxSegm+1) - t;

         int segment( floor(t) );

         if ((segment >= 0) && (segment <= maxSegm))
         {
            QPointF  control[4];
            QPointF  p[3];

            if ((_closedCurve) && (segment == (maxSegm-1)))
            {
               p[0] = _vertexVector[_vertexVector.size()-2];
               p[1] = _vertexVector.back();
               p[2] = _vertexVector[0];

               buildControlPoints( p[0], p[1], p[2], control, false, false );
            }
            else if ((_closedCurve) && (segment == maxSegm))
            {
               p[0] = _vertexVector.back();
               p[1] = _vertexVector[0];
               p[2] = _vertexVector[1];

               buildControlPoints( p[0], p[1], p[2], control, false, false );
            }
            else
            {
               p[0] = _vertexVector[segment  ];
               p[1] = _vertexVector[segment+1];
               p[2] = _vertexVector[segment+2];

               buildControlPoints( p[0], p[1], p[2], control, ((segment == 0) && (!_closedCurve)), (segment == maxSegm) );
            }
            return calcBezierTangent( t-double(segment), control );
         }
         else
         {
            if (t == (maxSegm+1))
            {
               QPointF  control[4];
               QPointF  p[3];

               if (_closedCurve)
               {
                  p[0] = _vertexVector[0];
                  p[1] = _vertexVector[1];
                  p[2] = _vertexVector[2];

                  buildControlPoints( p[0], p[1], p[2], control, true, false );
                  return calcBezierTangent( 0.0, control );
               }
               else
               {
                  p[0] = _vertexVector[maxSegm  ];
                  p[1] = _vertexVector[maxSegm+1];
                  p[2] = _vertexVector[maxSegm+2];

                  buildControlPoints( p[0], p[1], p[2], control, (maxSegm == 0), true );
                  return calcBezierTangent( 1.0, control );
               }
            }
         }
      }
      return QPointF(0.0,0.0);
   } /* SmoothLine::directionOnCurve */


   qreal SmoothLine::arcLength( qreal t) const
   {
      if (t < 0.0) return 0.0;

      QPointF                          control[4];
      QPointF                          p[3];
      QVector<QPointF>::const_iterator it = _vertexVector.begin();

      if (_closedCurve)
      {
         int i = 0;
         p[i++] = _vertexVector.back();
         while ((i<3) && (it != _vertexVector.end()))
         {
            p[i] = *it;
            ++it;
            ++i;
         }
         buildControlPoints( p[0], p[1], p[2], control, false, false );
      }
      else
      {
         int i = 0;
         while ((i<3) && (it != _vertexVector.end()))
         {
            p[i] = *it;
            ++it;
            ++i;
         }
         buildControlPoints( p[0], p[1], p[2], control, true, (it == _vertexVector.end()) );
      }

      if (t < 1.0) return simpsonApproximation( control, 0.0, t );

      double sum = simpsonApproximation( control, 0.0, 1.0 ); t -= 1.0;

      while (it != _vertexVector.end())
      {
         p[0] = p[1];
         p[1] = p[2];
         p[2] = *it;
         ++it;

         if (_closedCurve) buildControlPoints( p[0], p[1], p[2], control, false, false );
         else              buildControlPoints( p[0], p[1], p[2], control, false, (it == _vertexVector.end()) );

         if (t < 1.0)
         {
            sum += simpsonApproximation( control, 0.0, t );
            break;
         }
         else sum += simpsonApproximation( control, 0.0, 1.0 );
      }

      if ((t > 0.0) && (_closedCurve))
      {
         p[0] = p[1];
         p[1] = p[2];
         p[2] = _vertexVector.front();

         buildControlPoints( p[0], p[1], p[2], control, false, false );

         sum += simpsonApproximation( control, 0.0, t );
      }
      return sum;
   } /* SmoothLine::arcLength */


   HermiteSpline::HermiteSpline( const QVector<QPointF> & vertexVector
                               , const QVector<QPointF> & tangentVector, bool closedCurve )
   {
      HermiteSpline::vertexVector(vertexVector, tangentVector, closedCurve );
   } /* HermiteSpline::HermiteSpline */


   void HermiteSpline::vertexVector( const QVector<QPointF> & vertexVector
                                  , const QVector<QPointF> & tangentVector, bool closedCurve )
   {
      _closedCurve    = closedCurve;
      _vertexVector  = vertexVector;
      _tangentVector = tangentVector;

      int countOfTangents = (_closedCurve) ? _vertexVector.size()*2 : (_vertexVector.size()-1) * 2;

      if (_tangentVector.size() >= countOfTangents)
      {
         // fatal error
         throw
         SituationMap::Exception( __FILE__, __LINE__,
                    "No enough tangents defined to calculate spline. Each spline segment must have defined 2 tangents per vertex, excepting the border segments where only 1 tangent must be defined.");

      }
   } /* HermiteSpline::vertexVector */


   qreal HermiteSpline::maximumCurveParameter() const
   {
       if (_vertexVector.size() < 2) return 0.0;
       return (_closedCurve) ? _vertexVector.size() :  (_vertexVector.size() - 1.0);
   } /* HermiteSpline::maximumCurveParameter */


   QPointF HermiteSpline::positionOnCurve( qreal t ) const
   {
      if (_vertexVector.size() < 2) return QPointF(0.0,0.0);

      int  maxSegm = _vertexVector.size() - ((_closedCurve) ? 1 : 2);

      if (t < 0.0) t = double(maxSegm+1) - t;

      int segment( floor(t) );

      if ((segment >= 0) && (segment <= maxSegm))
      {
         QPointF  control[4];

         if ((_closedCurve) && (segment == maxSegm))
         {
            control[0] = _vertexVector.back();
            control[3] = _vertexVector.front();
            control[1] = control[0] + _tangentVector[_tangentVector.size()-2]/3.0;
            control[2] = control[3] - _tangentVector[_tangentVector.size()-1]/3.0;
         }
         else
         {
            control[0] = _vertexVector[segment];
            control[3] = _vertexVector[segment+1];
            control[1] = control[0] + _tangentVector[segment*2]/3.0;
            control[2] = control[3] - _tangentVector[segment*2+1]/3.0;
         }
         return calcBezierVertex( t-double(segment), control );
      }

      return QPointF(0.0,0.0);

   } /* HermiteSpline::positionOnCurve */


   QPointF HermiteSpline::directionOnCurve( qreal t ) const
   {
      if (_vertexVector.size() < 2)  return QPointF(0.0,0.0);

      int  maxSegm = _vertexVector.size() - ((_closedCurve) ? 1 : 2);

      if (t < 0.0) t = double(maxSegm+1) - t;

      int segment( floor(t) );

      if ((segment >= 0) && (segment <= maxSegm))
      {
         QPointF  control[4];

         if ((_closedCurve) && (segment == maxSegm))
         {
            control[0] = _vertexVector.back();
            control[3] = _vertexVector.front();
            control[1] = control[0] + _tangentVector[_tangentVector.size()-2]/3.0;
            control[2] = control[3] - _tangentVector[_tangentVector.size()-1]/3.0;
         }
         else
         {
            control[0] = _vertexVector[segment];
            control[3] = _vertexVector[segment+1];
            control[1] = control[0] + _tangentVector[segment*2]/3.0;
            control[2] = control[3] - _tangentVector[segment*2+1]/3.0;
         }
         return calcBezierTangent( t-double(segment), control );
      }

      return QPointF(0.0,0.0);

   } /* HermiteSpline::directionOnCurve */


   qreal HermiteSpline::arcLength( qreal t ) const
   {
      if (t < 0.0)                  return 0.0;
      if (_vertexVector.size() < 2) return 0.0;

      double     sum = 0.0;
      QPointF    control[4];

      QVector<QPointF>::const_iterator itv    = _vertexVector.begin();
      QVector<QPointF>::const_iterator itt    = _vertexVector.begin();

      if (_closedCurve) { if (2*_vertexVector.size()   != _tangentVector.size()) return 0.0; }
      else              { if (2*_vertexVector.size()-2 != _tangentVector.size()) return 0.0; }

      control[3] = *itv; ++itv;

      while (itv != _vertexVector.end())
      {
         control[0] = control[3];
         control[1] = control[0] + *itt / 3.0; ++itt;
         control[2] = *itv       - *itt / 3.0; ++itt;
         control[3] = *itv;

         if (t < 1.0) return sum + simpsonApproximation( control, 0.0, t );
         sum = simpsonApproximation( control, 0.0, 1.0 ); t -= 1.0;

         ++itv;
      } // while

      if (_closedCurve)
      {
         control[0] = control[3];
         control[1] = control[0]         + *itt / 3.0; ++itt;
         control[2] = _vertexVector.front() - *itt / 3.0; ++itt;
         control[3] = _vertexVector.front();

         if (t < 1.0) return sum + simpsonApproximation( control, 0.0, t );
         sum = simpsonApproximation( control, 0.0, 1.0 ); // t -= 1.0; // 'ScanBuild: 't is never read'
      }
      return sum;
   } /* HermiteSpline::arcLength */



   BezierSpline::BezierSpline( const QVector<QPointF> & ctrlPntVector, bool closedCurve )
      : HermiteSpline()
   {
      controlPointVector(ctrlPntVector,closedCurve);
   } /* BezierSpline::BezierSpline */


   void BezierSpline::controlPointVector( const QVector<QPointF> & ctrlPntVector, bool closedCurve )
   {
      if (ctrlPntVector.size() < 4) return;

      QVector<QPointF> vertexVector;
      QVector<QPointF> tangentVector;

      QVector<QPointF>::const_iterator it = ctrlPntVector.begin();

      vertexVector.append( *it );
      ++it;
      while (it != ctrlPntVector.end())
      {
         // set first tangent
         tangentVector.append( ((*it) - vertexVector.back()) * 3.0 );
         ++it;
         if (it == ctrlPntVector.end())
         {
            throw SituationMap::Exception(__FILE__, __LINE__, "Missing bezier spline tangent control point." );
         }
         ++it;

         if (it == ctrlPntVector.end())
         {
            // error
            if (closedCurve)
            {
               tangentVector.append( ((it[-1]) - vertexVector.front()) * 3.0 );
            }
            else throw SituationMap::Exception(__FILE__, __LINE__, "Missing bezier spline vertex control point." );
            break;
         }
         else
         {
            vertexVector.append( *it );
            tangentVector.append( ((it[-1]) - vertexVector.back()) * 3.0 );
            ++it;
         }
      } // while

      HermiteSpline::vertexVector(vertexVector,tangentVector,closedCurve);

   } /* BezierSpline::controlPointVector */


   void BezierSpline::vertexVector( const QVector<QPointF>&, const QVector<QPointF>&, bool )
   {
      throw SituationMap::Exception(__FILE__, __LINE__, "Function is not defined for BezierSpline. Use controlPointVector instead." );
   } /* BezierSpline::vertexVector */




   Ellipse::Ellipse( QPointF center, qreal rx, qreal ry, qreal phi, qreal sectorStart, qreal sectorEnd )
      : _center(center)
      , _radiusX(fabs(rx))
      , _radiusY(fabs(ry))
      , _sinPhi(sin(phi))
      , _cosPhi(cos(phi))
      , _phiSectorStart(sectorStart)
      , _phiSectorEnd(sectorEnd)
   {
      if (_radiusX < _radiusY)
      {
         // swap _radiusX and _radiusY
         _radiusX = fabs(ry);
         _radiusY = fabs(rx);

         // and rotate
         phi += MathConst::pi_half;
         _sinPhi = sin(phi);
         _cosPhi = cos(phi);
      }

      qreal a2 = _radiusX * _radiusX;
      qreal b2 = _radiusY * _radiusY;

      _eccentricity2 = (a2-b2) / a2;

      if (fabs(_phiSectorStart) > MathConst::pi_two) _phiSectorStart /= MathConst::pi_two;
      if (fabs(_phiSectorEnd)   > MathConst::pi_two) _phiSectorEnd   /= MathConst::pi_two;

      if (_phiSectorStart >= 0)
      {
         if (_phiSectorStart > _phiSectorEnd) _phiSectorStart -= MathConst::pi_two;
      }
      else
      {
         if (_phiSectorStart > _phiSectorEnd)_phiSectorEnd += MathConst::pi_two;
      }
   } /* Ellipse::Ellipse */


   qreal  Ellipse::maximumCurveParameter() const { return _phiSectorEnd;   }
   qreal  Ellipse::minimumCurveParameter() const { return _phiSectorStart; }

   QPointF Ellipse::positionOnCurve(qreal t) const
   {
      // Using ellipse equation in polar coordinates relative to center point(0,0)
      // because in this case the parameter t is identical with polar angle.

      qreal sin_t  = sin(t);
      qreal cos_t  = cos(t);

      /*
      qreal r = _radiusY / (1.0 - _eccentricity2 * cos_t*cos_t);
      qreal x = cos_t*r;
      qreal y = sin_t*r;
      */
      qreal x = cos_t*_radiusX;
      qreal y = sin_t*_radiusY;
      return _center + QPointF( x*_cosPhi - y*_sinPhi, y*_cosPhi + x*_sinPhi );

   } /* Ellipse::positionOnCurve */

   QPointF Ellipse::directionOnCurve(qreal t) const
   {
     qreal sin_t = sin(t);
     qreal cos_t = cos(t);

     if (cos_t == 0.0) return QPointF( -sin_t, 0.0 );

     qreal tan_t = sin_t / cos_t;
     qreal tan_n = tan_t / 1.0 - _eccentricity2; // tan normal

     // normal  QPointF( 1.0/sqrt(1+tan_n*tan_n), tan_n/sqrt(1+tan_n*tan_n) )
     QPointF tangente( -tan_n/sqrt(1+tan_n*tan_n), 1.0/sqrt(1+tan_n*tan_n) );

     return QPointF( tangente.x()*_cosPhi - tangente.y()*_sinPhi, tangente.y()*_cosPhi + tangente.x()*_sinPhi );
   } /* Ellipse::directionOnCurve */


   qreal  Ellipse::arcLength(qreal t) const
   {
     return arcLength( 0.0, t );
   } /* Ellipse::arcLength */


   qreal  Ellipse::arcLength(qreal tmin, qreal tmax ) const
   {
     Q_UNUSED(tmin);
     Q_UNUSED(tmax);

     // ToDo: calculate convergenting formula based on polar center points equation of ellipse if needed
     return 0.0;
   } /* Ellipse::arcLength */


   class CurvePathRasterer : public PathRasterer
   {
      public:

         CurvePathRasterer()                                         : PathRasterer(), _curveFunction(nullptr)        {}
         explicit CurvePathRasterer( const Curve * splineFunction )  : PathRasterer(), _curveFunction(splineFunction) {}
         ~CurvePathRasterer() = default;

         void function( qreal t, QPointF & result )  override
         {
            if (_curveFunction) result = _curveFunction->positionOnCurve(t);
            else
            {
               throw SituationMap::Exception(__FILE__, __LINE__, "Curve function is undefined for PathRasterer." );
            }
         } /* function */


      protected:

          const Curve * _curveFunction;

   }; /* CurvePathRasterer */


   PathRasterer * Ellipse::createPathRaster()       const { return new CurvePathRasterer(this); }
   PathRasterer * SmoothLine::createPathRaster()    const { return new CurvePathRasterer(this); }
   PathRasterer * HermiteSpline::createPathRaster() const { return new CurvePathRasterer(this); }
   PathRasterer * BezierSpline::createPathRaster()  const { return new CurvePathRasterer(this); }


} } // namespace SituationMap::MathUtils