/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#include <core/random.hpp>
#include <math/polygon.hpp>
#include <math/aabb2dpolygon.hpp>
#include <math/math.hpp>

#include <boost/random.hpp>
#include <cstdint>

namespace eXl
{
  class Serializer;

  class MathTools
  {
  public:

    //template <typename Real>
    //static Err Stream(Vector2<Real> const& iPoint, Serializer iStreamer);

    //template <typename Real>
    //static Err Unstream(Vector2<Real>& oPoint, Unstreamer& iUnstreamer);

    //template <typename Real>
    //static Err Stream(Polygon<Real> const& iPolygon, Streamer& iStreamer);
    //
    //template <typename Real>
    //static Err Unstream(Polygon<Real>& oPolygon, Unstreamer& iUnstreamer);
    //
    //template <typename Real>
    //static Err Stream(AABB2DPolygon<Real> const& iPolygon, Streamer& iStreamer);
    //
    //template <typename Real>
    //static Err Unstream(AABB2DPolygon<Real>& oPolygon, Unstreamer& iUnstreamer);

    //Clamp angle between 0 and 2*PI
    inline static float ClampAngle(float iAngle)
    {
      int div = iAngle / (Mathf::PI * 2);
      if(iAngle < 0)
      {
        --div;
      }
      return iAngle - div * Mathf::PI * 2;
    }

    // AngleDist between -PI and PI
    inline static float AngleDist(float iAngle1, float iAngle2)
    {
      float dist = ClampAngle(iAngle1) - ClampAngle(iAngle2);
      if(dist > Mathf::PI)
      {
        return 2.0 * Mathf::PI - dist;
      }
      else if(dist < -Mathf::PI)
      {
        return dist + 2.0 * Mathf::PI;
      }
      return dist;
    }

    template <typename Real>
    static inline Vector2<Real> Perp(Vector2<Real> const& iVec)
    {
      return Vector2<Real>(-iVec.Y(), iVec.X());
    }

    static inline uint64_t SquaredLength(Vector2i const& iVect)
    {
      return (int64_t)iVect.X() * (int64_t)iVect.X() + (int64_t)iVect.Y() * (int64_t)iVect.Y();
    }

    template <typename Real>
    static inline Vector2i ToIVec(Vector2<Real> const& iVec)
    {
      return Vector2i(Math<Real>::Round(iVec.X()), Math<Real>::Round(iVec.Y()));
    }

    template <typename Real>
    static inline Vector2f ToFVec(Vector2<Real> const& iVec)
    {
      return Vector2f(iVec.X(), iVec.Y());
    }

    template <typename Real>
    static inline Vector2d ToDVec(Vector2<Real> const& iVec)
    {
      return Vector2d(iVec.X(), iVec.Y());
    }

    template <typename Real>
    static inline Real Sign(Real iValue)
    {
      return iValue > 0 ? 1 : -1;
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, unsigned int &oSegNum, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true)
    {
      int dummyHole;
      Vector2<Real2> dummy;
      return NearestPointOnPoly(iPoint,iPoly, iMinSegLen, dummy, oSegNum, dummyHole, oPoint, oDir, iCheckExt, false);
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true)
    {
      int dummyHole;
      unsigned int dummy;
      return NearestPointOnPoly(iPoint,iPoly, iMinSegLen, oSeg, dummy, dummyHole, oPoint, oDir, iCheckExt, false);
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], unsigned int& oSegNum, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true)
    {
      int dummyHole;
      return NearestPointOnPoly(iPoint,iPoly, iMinSegLen, oSeg, oSegNum, dummyHole, oPoint, oDir, iCheckExt, false);
    }

    template <typename Real1, typename Real2>
    static inline bool NearestPointOnPoly(Vector2<Real1> const& iPoint, Polygon<Real2> const& iPoly, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], unsigned int& oSegNum, int& oHole, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, bool iCheckExt = true, bool iCheckHoles = false)
    {
      Real1 minDist;
      oHole = -1;
      bool res = _NearestPointOnPoly(iPoint, iPoly.Border(), iMinSegLen, oSeg, oSegNum, oPoint, oDir, minDist, iCheckExt);
      if(iCheckHoles)
      {
        for(unsigned int i = 0 ; i< iPoly.Holes().size(); ++i)
        {
          unsigned int segNum;
          Vector2<Real1> point;
          Vector2<Real2> seg[2];
          Vector2<Real1>    dir;
          Real1 curMin;
          bool newRes = _NearestPointOnPoly(iPoint, iPoly.Holes()[i], iMinSegLen, seg, segNum, point, dir, curMin, iCheckExt);
          if(newRes)
          {
            if(!res || curMin < minDist)
            {
              oHole = i;
              res = newRes;
              oSegNum = segNum;
              oPoint = point;
              oDir = dir;
              oSeg[0] = seg[0];
              oSeg[1] = seg[1];
              minDist = curMin;
            }
          }
        }
      }
      return res;
    }

    template <typename Real1, typename Real2>
    static inline bool _NearestPointOnPoly(Vector2<Real1> const& iPoint, Vector<Vector2<Real2> > const& polyBorder, Real1 iMinSegLen, Vector2<Real2> (&oSeg)[2], unsigned int& oSegNum, Vector2<Real1>& oPoint, Vector2<Real1>& oDir, Real1& oDist, bool iCheckExt)
    {
      Vector2<Real1> polyCenter;
      for(auto point : polyBorder)
      {
        polyCenter += Vector2<Real1>(point.X(), point.Y());
      }

      if(polyBorder.front() == polyBorder.back())
      {
        polyCenter -= Vector2<Real1>(polyBorder.front().X(), polyBorder.front().Y());
        polyCenter = polyCenter / (polyBorder.size() - 1);
      }
      else
      {
        polyCenter = polyCenter / polyBorder.size();
      }

      //Vector2<Real1> iDir = polyCenter - iPoint;
      //Real1 minDist = iDir.Normalize();

      Vector2<Real1> prevPoint(polyBorder.back().X(), polyBorder.back().Y());

      Vector2<Real1> iDir = polyCenter - iPoint;
      iDir.Normalize();
      Real1 minDist = (iPoint - prevPoint).Length() + 1;

      for (unsigned int i = 0; i < polyBorder.size(); ++i)
      {
        Vector2<Real1> curPoint(polyBorder[i].X(), polyBorder[i].Y());

        if (curPoint != prevPoint)
        {
          Vector2<Real1> dir = curPoint - prevPoint;
          Vector2<Real1> extDir(MathTools::Perp(dir));

          if (!iCheckExt || (iDir).Dot(extDir) < 0)
          {
            Real1 segLen = dir.Normalize();
            if (segLen > iMinSegLen)
            {
              Vector2<Real1> prevPointOff = prevPoint + dir * 0.5 * iMinSegLen;
              Vector2<Real1> curPointOff = curPoint - dir * 0.5 * iMinSegLen;
              segLen -= iMinSegLen;
              Real1 dirPoj = (iPoint - prevPointOff).Dot(dir);
              Vector2<Real1> candidate;

              if (dirPoj < -Math<Real1>::ZERO_TOLERANCE)
              {
                candidate = prevPointOff;
              }
              else if (dirPoj > segLen - Math<Real1>::ZERO_TOLERANCE)
              {
                candidate = curPointOff;
              }
              else
              {
                candidate = prevPointOff + (dir * dirPoj);
              }

              Real1 curDist = (candidate - iPoint).Length();
              if (curDist < minDist)
              {
                oSeg[0] = Vector2<Real2>(prevPoint.X(), prevPoint.Y());
                oSeg[1] = Vector2<Real2>(curPoint.X(), curPoint.Y());
                if(i == 0)
                  oSegNum = polyBorder.size() - 1;
                else
                  oSegNum = i - 1;
                oPoint = candidate;
                oDir = extDir;
                oDir.Normalize();
                minDist = curDist;
              }
            }
          }
        }

        prevPoint = curPoint;
      }

      oDist = minDist;

      if(minDist != (iPoint - polyCenter).Length())
        return true;
      else
        return false;
    }

    template <typename Real>
    static void SimplifyPolygon(Polygon<Real> const& iPoly1, double iEpsilon, Polygon<Real>& oPoly);

    template <typename Real1, typename Real2>
    static inline unsigned int FindPolyIntersection(Vector2<Real1> const& iPoint1, Vector2<Real1> const& iPoint2, Polygon<Real2> const& iPoly, Vector2<Real1>(&oPoints)[2])
    {
      unsigned int curInter = 0;
      Vector<Vector2<Real2> > const& border = iPoly.Border();
      Vector2<Real1> prevPoint(border.back().X(), border.back().Y());
      for (unsigned int i = 0; i < border.size() && curInter < 2; ++i)
      {
        Vector2<Real1> curPoint(border[i].X(), border[i].Y());

        unsigned int res = Segment<Real1>::Intersect(prevPoint, curPoint, iPoint1, iPoint2, oPoints[curInter]);
        if (res & Segmentd::PointOnSegment1)
        {
          if ((curInter == 0 || (oPoints[0] - oPoints[curInter]).Length() > Math<Real1>::ZERO_TOLERANCE))
          {
            ++curInter;
          }
        }
        prevPoint = curPoint;
      }
      return curInter;
    }

    template <typename Real1, typename Real2>
    static bool FindCommonSegment(Polygon<Real2> const& iPoly1, Polygon<Real2> const& iPoly2, Vector2<Real1>& oSegPt1, Vector2<Real1>& oSegPt2, Real1 iMinLen = Math<Real1>::ZERO_TOLERANCE)
    {
      bool foundCommon = false;
      Vector2<Real1> prevPt1 = Vector2<Real1>(iPoly1.Border().back().X(),iPoly1.Border().back().Y());
      for(auto curPt1I : iPoly1.Border())
      {
        Vector2<Real1> curPt1 = Vector2<Real1>(curPt1I.X(),curPt1I.Y());
        if(prevPt1 != curPt1)
        {
          Vector2<Real1> prevPt2 = Vector2<Real1>(iPoly2.Border().back().X(), iPoly2.Border().back().Y());
          for(auto curPt2I : iPoly2.Border())
          {
            Vector2<Real1> curPt2 = Vector2<Real1>(curPt2I.X(),curPt2I.Y());
            if(prevPt2 != curPt2)
            {
                unsigned int res = Segment<Real1>::Intersect(prevPt1, curPt1, prevPt2, curPt2);
                if(res & Segment<Real1>::AlignedSegments
                && (res & Segment<Real1>::ConfoundSegments || res & Segmentd::PointOnSegments))
                {
                  Vector2<Real1> dir = curPt1 - prevPt1;
                  AABB2D<Real1> box1(0.0,0.0, dir.Normalize(), 0.0);
                  double val1 = (prevPt2 - prevPt1).Dot(dir);
                  double val2 = (curPt2 - prevPt1).Dot(dir);
                  AABB2D<Real1> box2 = val1 > val2 ? AABB2D<Real1>(val2, 0.0, val1, 0.0) : AABB2D<Real1>(val1, 0.0, val2, 0.0);
                  AABB2D<Real1> result;
                  result.SetCommonBox(box1, box2);
                  double commonLen = result.m_Data[1].X() - result.m_Data[0].X();
                  if(commonLen >= iMinLen)
                  {
                    foundCommon = true;
                    oSegPt1 = prevPt1 + dir * result.m_Data[0].X();
                    oSegPt2 = prevPt1 + dir * (result.m_Data[0].X() + commonLen);
                    break;
                  }
                }
            }
            prevPt2 = curPt2;
          }
        }
        if(foundCommon)
          break;
        prevPt1 = curPt1;
      }
      return foundCommon;
    }

    template <typename Real>
    static inline Real GetAngleFromVec(Vector2<Real> const& iDir)
    {
      return Math<Real>::Abs(iDir.X()) < Math<Real>::EPSILON ? (iDir.Y() > 0.0 ? Math<Real>::PI * 0.5 : Math<Real>::PI * 1.5)
        : (iDir.X() > 0.0 ? Math<Real>::ATan(iDir.Y() / iDir.X()) : Math<Real>::PI - Math<Real>::ATan(iDir.Y() / (-1.0 * iDir.X())));
    }

    template<typename Real>
    static inline Vector2<Real> GetPerp(Vector2<Real> const& iVec)
    {
      return Vector2<Real>(iVec.Y() * -1.0, iVec.X());
    }

    template<typename Real>
    static inline Vector2<Real> GetLocal(Vector2<Real> const& iBase, Vector2<Real> const& iVec)
    {
      return Vector2<Real>(iBase.X() * iVec.X() + iBase.Y() * iVec.Y(), iBase.X() * iVec.Y() - iBase.Y() * iVec.X());
    }

    template<typename Real>
    static inline Vector2i RoundVector(Vector2<Real> const& iVec)
    {
      return Vector2i(Math<Real>::Round(iVec.X()), Math<Real>::Round(iVec.Y()));
    }

    template <typename Real>
    static inline Real ModAngle(Real iAngle)
    {
      iAngle = fmod(iAngle, 2.0*Math<Real>::PI);
      iAngle -= Mathf::PI;
      return iAngle;
    }

    template <typename Real>
    static inline Real RandFloatIn(Random& randGen, Real iMin, Real iMax)
    {
      return Real(randGen() % 10000) / 10000.0 * (iMax - iMin) + iMin;
    };

    template <typename Real>
    static inline Real RandNormFloatIn(Random& randGen, Real iMin, Real iMax)
    {
      boost::random::normal_distribution<Real> distrib;
      RandomWrapper randW(&randGen);
      Real rangeValNorm = distrib(randW);
      Real rangeVal = rangeValNorm * ((iMax-iMin) * 0.33);
      Real offsetVal = rangeVal + (iMax + iMin) * 0.5;

      return Math<Real>::Clamp( offsetVal , iMin, iMax);
    };

    static inline int RandIntIn(Random& randGen, int iMin, int iMax)
    {
      return (randGen() % (iMax - iMin)) + iMin;
    };

    static inline Vector2f Perturbate(Random& iRand, Vector2f const& iPos, float iRadius)
    {
      float const angle = ((iRand.Generate() % 10000) / 10000.0 - 1.0) * Mathf::PI;
      float const dist = ((iRand.Generate() % 10000) / 10000.0 ) * iRadius;

      return iPos + Vector2f(Mathd::Cos(angle), Mathd::Sin(angle)) * dist;
    }

    template <typename Real>
    static inline Vector2<Real> const& As2DVec(Vector3<Real> const& iVec)
    {
      return reinterpret_cast<Vector2<Real> const&>(iVec);
    }

    template <typename Real>
    static inline Vector2<Real>& As2DVec(Vector3<Real>& iVec)
    {
      return reinterpret_cast<Vector2<Real>&>(iVec);
    }

    template <typename Real>
    static inline Vector3<Real> To3DVec(Vector2<Real> const& iVec, Real iZComp = 0)
    {
      return Vector3<Real>(iVec.X(), iVec.Y(), iZComp);
    }

    template <typename Real>
    static inline Vector3<Real> const& GetPosition(Matrix4<Real> const& iMat)
    {
      return *reinterpret_cast<Vector3<Real> const*>(iMat.m_Data + 12);
    }

    template <typename Real>
    static inline Vector2<Real> const& GetPosition2D(Matrix4<Real> const& iMat)
    {
      return *reinterpret_cast<Vector2<Real> const*>(iMat.m_Data + 12);
    }

    template <typename Real>
    static inline Vector3<Real>& GetPosition(Matrix4<Real>& iMat)
    {
      return *reinterpret_cast<Vector3<Real>*>(iMat.m_Data + 12);
    }

    template <typename Real>
    static inline Vector2<Real>& GetPosition2D(Matrix4<Real>& iMat)
    {
      return *reinterpret_cast<Vector2<Real>*>(iMat.m_Data + 12);
    }

    // Prereq : points have to be in CCW order
    template <typename Real>
    static Vector2<Real> ConeGetMidSegment(Vector2<Real> const (&iRange)[2])
    {
      Vector2<Real> mid = iRange[0] + iRange[1];
      Real dist = mid.Normalize();
      if (dist < Math<Real>::ZERO_TOLERANCE)
      {
        mid = GetPerp(iRange[0]);
      }
      mid *= Math<Real>::Sign(Segment<Real>::Cross(iRange[0], mid));

      return mid;
    }

    template <typename Real>
    static void ConeUpdateRange(Vector2<Real> const (&iRange)[2], Vector2<Real>& oMid, Real& oLowLimit)
    {
      oMid = ConeGetMidSegment(iRange);
      oLowLimit = iRange[0].Dot(oMid) / iRange[0].Length();
    }

    template <typename Real>
    static bool IsInCone(Vector2<Real> const& iDir, Vector2<Real> const& iMidSeg, Real iLowLimit, Real iEpsilon = Math<Real>::EPSILON)
    {
      return iDir.Dot(iMidSeg) > (iLowLimit - iEpsilon);
    }

    template <typename Real>
    static Vector3<Real> Reflect(Vector3<Real> const& iIncomingDir, Vector3<Real> const& iNormal)
    {
      return iIncomingDir - iNormal * iIncomingDir.Dot(iNormal) * 2;
    }

    template <typename Real>
    static void ClampToBox(Vector2<Real>& ioPoint, AABB2D<Real> const& iBox)
    {
      for (uint32_t i = 0; i < 2; ++i)
      {
        ioPoint.m_Data[i] = Math<Real>::Clamp(ioPoint.m_Data[i], iBox.m_Data[0].m_Data[i], iBox.m_Data[1].m_Data[i]);
      }
    }

    //static void AlignVector(Vector2d& ioDir, Vector2i& oCanonicalVector, unsigned int iFactor)
    //{
    //  if (Mathd::Abs(ioDir.X()) > Mathd::ZERO_TOLERANCE)
    //  {
    //    if (Mathd::Abs(ioDir.Y()) > Mathd::ZERO_TOLERANCE)
    //    {
    //      double ratio = (ioDir.X() / ioDir.Y()));
    //      int sign = Sign(ratio);
    //      ratio = ratio * sign;
    //      if (ratio > 1)
    //      {
    //        ratio = 1 / ratio;
    //        unsigned int ratioI = Mathi::Round(ratio * iFactor);
    //        oCanonicalVector = Vector2i(Sign(ioDir.X()) * iFactor, Sign(ioDir.Y()) * ratioI);
    //      }
    //      else
    //      {
    //        unsigned int ratioI = Mathi::Round(ratio * iFactor);
    //        oCanonicalVector = Vector2i(Sign(ioDir.X()) * ratioI, Sign(ioDir.Y()) * iFactor);
    //      }
    //      ioDir = MathTools::ToDVec(oCanonicalVector) * ioDir.Length() / Mathd::Sqrt(ratioI*ratioI + iFactor*iFactor);
    //      return;
    //    }
    //  }
    //  ioDir = Vector2d::ZERO;
    //  oCanonicalVector = Vector2i::ZERO;
    //}
    //
    //static void AlignSegment(Vector2d& ioPoint1, Vector2d ioPoint2, unsigned int iFactor)
    //{
    //
    //}

  };

  //template <>
  //EXL_MATH_API Err MathTools::Stream<float>(Vector2f const& iPoint, Serializer iStreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Unstream<float>(Vector2f& oPoint, Unstreamer& iUnstreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Stream<int>(Vector2i const& iPoint, Serializer iStreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Unstream<int>(Vector2i& oPoint, Unstreamer& iUnstreamer);

  //template <>
  //EXL_MATH_API Err MathTools::Stream<int>(Polygoni const& iPoint, Streamer& iStreamer);
  //
  //template <>
  //EXL_MATH_API Err MathTools::Unstream<int>(Polygoni& oPoint, Unstreamer& iUnstreamer);
  //
  //template <>
  //EXL_MATH_API Err MathTools::Stream<int>(AABB2DPolygoni const& iPoint, Streamer& iStreamer);
  //
  //template <>
  //EXL_MATH_API Err MathTools::Unstream<int>(AABB2DPolygoni& oPoint, Unstreamer& iUnstreamer);

  template <>
  EXL_MATH_API void MathTools::SimplifyPolygon<int>(Polygoni const& iPoly1, double iEpsilon, Polygoni& oPoly);
  //template <>
  //EXL_GEN_API void MathTools::SimplifyPolygon<float>(Polygonf const& iPoly1, double iEpsilon, Polygonf& oPoly);
  //template <>
  //EXL_GEN_API void MathTools::SimplifyPolygon<double>(Polygond const& iPoly1, double iEpsilon, Polygond& oPoly);

  //template <typename Real>
  //struct StreamerTemplateHandler<Polygon<Real>>
  //{
  //  static Err Do(Streamer& iStreamer, Polygon<Real> const* iObj)
  //  {
  //    return MathTools::Stream<Real>(*iObj, iStreamer);
  //  }
  //};
  //
  //template <typename Real>
  //struct UnstreamerTemplateHandler<Polygon<Real>>
  //{
  //  static Err Do(Unstreamer& iUnstreamer, Polygon<Real>* iObj)
  //  {
  //    return MathTools::Unstream<Real>(*iObj, iUnstreamer);
  //  }
  //};

  EXL_MATH_API uint32_t Factorial(uint32_t i);

  struct EXL_MATH_API CombinationHelper_Iter
  {
    // Compute K amongst N with as an iterator adaptor.
    CombinationHelper_Iter(uint32_t iSize, uint32_t iNum, bool iIsEnd);

    void ComputeMaxStep(uint32_t iSize, uint32_t iNum);

    Vector<uint32_t> const& operator*() const { return m_Stack; }

    CombinationHelper_Iter& operator ++()
    {
      Advance();
      return *this;
    }

    bool operator == (CombinationHelper_Iter const& iOther) const
    {
      return m_Step == iOther.m_Step;
    }
    bool operator != (CombinationHelper_Iter const& iOther) const
    {
      return !(*this == iOther);
    }

    void Advance();

    Vector<uint32_t> m_Stack;
    uint32_t m_Size;
    uint32_t m_Num;
    uint32_t m_Step;
    uint32_t m_MaxStep;
  };

  struct CombinationHelper
  {
    CombinationHelper(uint32_t iSize, uint32_t iNum)
      : m_Size(iSize)
      , m_Num(iNum)
    {

    }

    CombinationHelper_Iter begin() const { return CombinationHelper_Iter(m_Size, m_Num, false); }
    CombinationHelper_Iter end() const { return CombinationHelper_Iter(m_Size, m_Num, true); }

    uint32_t m_Size;
    uint32_t m_Num;
  };

  template <typename T>
  struct TCombinationHelper_Set_Iter
  {
    TCombinationHelper_Set_Iter(Vector<T> const& iElements, CombinationHelper_Iter const& iIter, bool iEnd)
      : m_Elements(iElements)
      , m_Iter(iIter)
    {
      if (iEnd)
      {
        m_Cur = iIter.m_Num;
      }
      else
      {
        m_Cur = 0;
      }
    }

    T const& operator*()
    {
      return m_Elements[(*m_Iter)[m_Cur]];
    }

    TCombinationHelper_Set_Iter& operator ++()
    {
      ++m_Cur;
      return *this;
    }

    bool operator == (TCombinationHelper_Set_Iter const& iOther) const
    {
      return m_Cur == iOther.m_Cur;
    }
    bool operator != (TCombinationHelper_Set_Iter const& iOther) const
    {
      return !(*this == iOther);
    }

    uint32_t m_Cur;
    Vector<T> const& m_Elements;
    CombinationHelper_Iter const& m_Iter;
  };

  template <typename T>
  struct TCombinationHelper_Set
  {
    TCombinationHelper_Set(Vector<T> const& iElements, CombinationHelper_Iter const& iIter)
      : m_Elements(iElements)
      , m_Iter(iIter)
    {

    }

    TCombinationHelper_Set_Iter<T> begin() const { return TCombinationHelper_Set_Iter<T>(m_Elements, m_Iter, false); }
    TCombinationHelper_Set_Iter<T> end() const { return TCombinationHelper_Set_Iter<T>(m_Elements, m_Iter, true); }

    Vector<T> const& m_Elements;
    CombinationHelper_Iter const& m_Iter;
  };

  template <typename T>
  struct TCombinationHelper_Iter
  {
    TCombinationHelper_Iter(Vector<T> const& iElements, uint32_t iNum, bool iEnd)
      : m_Elements(iElements)
      , m_Iter(iElements.size(), iNum, iEnd)
    {

    }

    TCombinationHelper_Set<T> operator*()
    {
      return TCombinationHelper_Set<T>(m_Elements, m_Iter);
    }

    TCombinationHelper_Iter& operator ++()
    {
      ++m_Iter;
      return *this;
    }

    bool operator == (TCombinationHelper_Iter const& iOther) const
    {
      return m_Iter == iOther.m_Iter;
    }
    bool operator != (TCombinationHelper_Iter const& iOther) const
    {
      return !(*this == iOther);
    }

    Vector<T> const& m_Elements;
    CombinationHelper_Iter m_Iter;
  };

  template <typename T>
  struct TCombinationHelper
  {
    TCombinationHelper(Vector<T> const& iElements, uint32_t iNum)
      : m_Elements(iElements)
      , m_Num(iNum)
    {

    }

    TCombinationHelper_Iter<T> begin() const { return TCombinationHelper_Iter<T>(m_Elements, m_Num, false); }
    TCombinationHelper_Iter<T> end() const { return TCombinationHelper_Iter<T>(m_Elements, m_Num, true); }

    Vector<T> const& m_Elements;
    uint32_t m_Num;
  };
}


// The code is open source under the MIT license.
// Copyright 2019-2020, Phillip Keldenich, TU Braunschweig, Algorithms Group
// https://ibr.cs.tu-bs.de/alg
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to
// use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is furnished to do
// so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

//
// Created by Phillip Keldenich on 2019-09-23.
//

#pragma once

#include <gmpxx.h>
#include <mpfr.h>

#include <type_traits>
#include <iostream>
#include <sstream>
#include <fstream>
#include <utility>
#include <limits>
#include <climits>
#include <vector>

#include <cassert>
#include <climits>
#include <cstddef>
#include <cstdint>
#include <cmath>

#include <boost/io/ios_state.hpp>
#include <boost/config.hpp>

#include "ivarp/rounding.hpp"
#include "ivarp/bool.hpp"
#include "ivarp/metaprogramming.hpp"
#include "number/fwd.hpp"
#include "number/device_compat.hpp"
#include "number/exact_less_than.hpp"

namespace ivarp {
    /// A traits template that defines methods for getting lower and
    /// upper bounds as well as possible/definite undefinedness.
    template<typename NT> struct NumberTraits {
        static constexpr bool is_number = false;
    };

    /// A type and an object that can be passed to intervals to set the corresponding bound to infinity;
    /// needed for intervals if the underlying number type has no infinities.
    struct InfinityType {
        constexpr IVARP_HD InfinityType operator-() const noexcept { return InfinityType{}; }
    };

    static IVARP_CUDA_DEVICE_OR_CONSTEXPR InfinityType infinity{};

    template<typename Number> IVARP_HD constexpr static inline std::enable_if_t<!std::is_floating_point<Number>::value, bool>
        is_finite(const Number&) noexcept
    {
        return true;
    }

    template<typename Number> IVARP_HD static inline std::enable_if_t<std::is_floating_point<Number>::value, bool>
        is_finite(Number n) noexcept
    {
        return IVARP_NOCUDA_USE_STD isfinite(n);
    }

    template<typename Number> IVARP_HD static inline bool possibly_undefined(const Number& n) {
        return NumberTraits<Number>::possibly_undefined(n);
    }

    template<typename Number> IVARP_HD static inline bool definitely_defined(const Number& n) {
        return !NumberTraits<Number>::possibly_undefined(n);
    }

    template<typename Number>
        static inline IVARP_HD std::enable_if_t<NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> lb(const Number& n)
    {
        return NumberTraits<Number>::lb(n);
    }

    template<typename Number>
        static inline IVARP_H std::enable_if_t<!NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> lb(const Number& n)
    {
        return NumberTraits<Number>::lb(n);
    }

    template<typename Number>
        static inline IVARP_HD std::enable_if_t<NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> ub(const Number& n)
    {
        return NumberTraits<Number>::ub(n);
    }

    template<typename Number>
        static inline IVARP_H std::enable_if_t<!NumberTraits<Number>::allows_cuda, typename NumberTraits<Number>::BoundType> ub(const Number& n)
    {
        return NumberTraits<Number>::ub(n);
    }

    template<typename IntType> using IsIntegral = std::integral_constant<bool,
        std::is_integral<BareType<IntType>>::value || std::is_same<BareType<IntType>, BigInt>::value
    >;

    template<typename T> struct IsRationalImpl : std::false_type {};
    template<typename A> struct IsRationalImpl<__gmp_expr<mpq_t, A>> : std::true_type {};

    template<typename NumType> using IsNumber = std::integral_constant<bool,
        NumberTraits<BareType<NumType>>::is_number>;

    template<bool IsNumber, typename NumType> struct IsCUDANumberImpl : std::false_type {};
    template<typename NumType> struct IsCUDANumberImpl<true, NumType> : std::integral_constant<bool, NumberTraits<NumType>::allows_cuda> {};
    template<typename NumType> using IsCUDANumber = IsCUDANumberImpl<IsNumber<NumType>::value, NumType>;

    template<typename T> using IsIntOrRational = std::integral_constant<bool,
            IsIntegral<T>::value || IsRational<T>::value>;

    template<typename T, bool B = IsNumber<T>::value> struct IsIntervalType : std::false_type {};
    template<typename T> struct IsIntervalType<T,true> : std::integral_constant<bool, NumberTraits<T>::is_interval> {};

    /// An interval class that also keeps track of definedness issues.
    template<typename NumberType> class Interval;

    // Intervals of float, double and Rational type.
    using IFloat  = Interval<float>;
    using IDouble = Interval<double>;
    using IRational = Interval<Rational>;

    /**
     * @brief Generate a (canonicalized) rational number from one or two integers.
     * Compared to the native GMP interface, this provides two benefits:
     * - It canonicalizes the number; failure to do so can results in difficult-to-diagnose bugs.
     * - It works with _all_ integral types including [unsigned] long long and std::[u]int64_t.
     * There also are versions where one or both of the arguments are BigInt.
     * @return The rational
     */
    template<typename IntegralType1, std::enable_if_t<IsIntegral<IntegralType1>::value,int> = 0>
        static inline Rational rational(IntegralType1 num);
    template<typename IntegralType1, typename IntegralType2>
        static inline std::enable_if_t<IsIntegral<IntegralType1>::value && IsIntegral<IntegralType2>::value, Rational>
            rational(IntegralType1 num, IntegralType2 denom);

    // a metafunction that computes the promoted number type for a set of argument types:
    // computations are done, unless additional promotions to intervals occur due to some operations, using
    // the number type with the highest rank between all arguments.
    // If a promotion to intervals occurs, it uses the interval version of the basic number type.
    // Rank is determined as follows:
    //  - Any interval type has higher rank than any number type. In particular, passing one float interval and
    //    a rational parameter leads to the computation being done on float intervals.
    //  - More precise number types have higher rank.
    template<typename Arg1, typename... Args> struct NumberTypePromotion;
    template<typename... Args> using Promote = typename NumberTypePromotion<BareType<Args>...>::type;
//  template<typename TargetType, typename SourceType> static inline
//      TargetType convert_number(const SourceType& source);
}

#include "number/mpfr.hpp"
#include "number/interval.hpp"
#include "number/traits.hpp"
#include "number/rational.hpp"
#include "number/fixed_point_bounds.hpp"
#include "number/fixed_point_bounds_sqrt.hpp"
#include "number/fixed_point_bounds_sin.hpp"
#include "number/fixed_point_bounds_cos.hpp"
#include "number/float_interval_ops.hpp"
#include "number/interval_comparisons.hpp"
#include "number/bounded_rational.hpp"
#include "number/type_conversions.hpp"
#include "number/minmax.hpp"
#include "number/literals.hpp"
#include "number/factorize.hpp"


/*
 *  The MIT License (MIT)
 *
 *  Copyright (c) 2015 MRL-SPL RoboCup Team
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in all
 *  copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *  SOFTWARE.
 *
 */

/*
 * File:   Chain.cpp
 * Author: Mohammad Ali Sharpasand
 *
 * Created on March 17, 2016
 */

#include "Chain.hpp"
#include "Angle.hpp"
#include "Utility.hpp"

#include <stdexcept>
#include <vector>
#include <armadillo>
#include <ceres/ceres.h>
using namespace BipedLibrary;
using namespace BipedLibrary::Utility;
using ceres::CostFunction;
using ceres::SizedCostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

Chain::Chain(vec3 const posi_body_body, mat33 const ori_body) :
posi_body_body_(posi_body_body),
ori_body_(ori_body)
{

}

vec3 Chain::posi_body_body(int frame) const
{
    return ori_body_ * position_base_base(frame) + posi_body_body_;
}

mat33 const& Chain::ori_body() const
{
    return ori_body_;
}

vec3 Chain::posi_end_last_last() const
{
	return posi_end_last_last_;
}
mat33 Chain::ori_end_last() const
{
	return ori_end_last_;
}

Chain& Chain::setPosi_body_body(vec3 const& posi_body_body)
{

    posi_body_body_ = posi_body_body;
    return *this;
}

Chain& Chain::setOri_body(mat33 const& ori_body)
{
    ori_body_ = ori_body;
    return *this;
}

Chain& Chain::setPosi_end_last_last(vec3 const& posi_end_last_last)
{
	posi_end_last_last_ = posi_end_last_last;
	return *this;
}

Chain& Chain::setOri_end_last(mat33 const& ori_end_last)
{
	ori_end_last_ = ori_end_last;
	return *this;
}

vec3 Chain::position_end_body_body() const
{
	mat33 const ori_last_base = orientation_base();
	vec3 const posi_end_base_base = position_base_base() + ori_last_base * posi_end_last_last_;
	return posi_body_body_ + ori_body_ * posi_end_base_base;
}

vec3 Chain::position_base_base(int frame) const
{
    if(frame == -1)
        frame = size() - 1;

    if(frame < 0 || (size_t)frame >= size())
        throw(std::out_of_range("Index out of range in BipedLibrary::Chain::position_base_base(int)"));

    if(frame == 0)
        return position_pre_pre(frame);

    return position_base_base(frame - 1) + orientation_base(frame - 1) * position_pre_pre(frame);
}

vec3 Chain::position_body_body(int frame) const
{
	return posi_body_body_ + ori_body_ * position_base_base(frame);
}

vec3 Chain::position_pre_pre(int i) const
{
    vec3 output;

    try
    {
        output <<   at(i).r()                       << endr
               << - at(i).d() * at(i).alpha().sin() << endr
               <<   at(i).d() * at(i).alpha().cos() << endr;
    }
    catch(std::out_of_range &)
    {
        throw(std::out_of_range("Out of range index in Chain::position_pre_pre(int)."));
    }

    return output;
}

vec3 Chain::position_com(int frame) const
{
    // FIXME: This is not actually COM! Be warned!
    return vec3({at(frame).r() / 2.f, 0.f, 0.f});
}

mat33 Chain::orientation_end_body() const
{
	return ori_body_ * orientation_base() * ori_end_last_;

}

mat33 Chain::orientation_base(int frame) const
{
    if(frame == -1)
        frame = size() - 1;

    if(frame < 0 || (size_t)frame >= size())
        throw(std::out_of_range("Index out of range in BipedLibrary::Chain::orientation_base(int)"));

    if (frame == 0)
        return orientation_pre(frame);
    else
        return orientation_base(frame-1) * orientation_pre(frame);

}

mat33 Chain::orientation_body(int frame) const
{
	return ori_body_ * orientation_base(frame);
}

// TODO: This should be in DHFrame class
mat33 Chain::orientation_pre(int i) const
{
    mat33 output;
    Angle theta,alpha;
    theta = at(i).theta();
    alpha = at(i).alpha();
    try
    {
        output  << theta.cos()               << -theta.sin()              << 0            << endr
                << theta.sin() * alpha.cos() << theta.cos() * alpha.cos() << -alpha.sin() << endr
	            << theta.sin() * alpha.sin() << theta.cos() * alpha.sin() <<  alpha.cos() << endr;
    }
    catch(std::out_of_range &)
    {
        throw(std::out_of_range("Out of range index in Chain::orientation_pre(int)."));
    }

    return output;

}

mat66 Chain::jacobi_base(int frame, vec3 target) const
{
    mat66 jacobi = zeros(6, 6);
    vec3 const z = {0, 0, 1};
    for (int i=0; i<=frame; i++)
    {
        vec3 const z_i_base = orientation_base(i) * z;
        vec3 const r_frame_i_base = position_base_base(frame) - position_base_base(i) + orientation_base(frame) * target;
        jacobi.col(i).rows(0, 2) = crossProductMatrix(z_i_base) * r_frame_i_base;
        jacobi.col(i).rows(3, 5) = z_i_base;
    }

    return jacobi;
}

mat66 Chain::jacobi_body(int frame, vec3 target) const
{
	mat66 doubleOrientation = zeros(6, 6);
	doubleOrientation(span(0, 2), span(0, 2)) = ori_body();
	doubleOrientation(span(3, 5), span(3, 5)) = ori_body();

	return doubleOrientation * jacobi_base(frame, target);
}

bool Chain::solveForJointAngles(vec3 const& posi_end_body, mat33 ori_end_body)
{
//	google::InitGoogleLogging("");

	class IKCostFunction : public CostFunction
	{
	 public:
		IKCostFunction(Chain& chain, vec3 const& posi_end_body, mat33 ori_end_body)
	 	 : chain_(chain),
		   posi_end_body_(posi_end_body),
		   ori_end_body_(ori_end_body)
	 {
		mutable_parameter_block_sizes()->push_back(chain.size());

	   set_num_residuals(6);
	 }

	  virtual ~IKCostFunction() {}

	  virtual bool Evaluate(double const* const* parameters,
	                        double* residuals,
	                        double** jacobians) const
	  {
		  int i = 0;
		  for(DHFrame& frame : chain_)
		  {
			  frame.setTheta(parameters[0][i]);
			  i++;
		  }

		  vec3 const posiRes = chain_.position_end_body_body() - posi_end_body_;
		  for(int i = 0; i < 3; i++)
			  residuals[i] = posiRes(i);

		  mat33 const  oriResMat = ori_end_body_.t() * chain_.orientation_end_body();
		  vec3 const oriRes = Utility::rotationMatrixToAxisAngle(oriResMat).asAVector();

//		  vec3 const oriRes = Utility::rotationMatrixToAxisAngle(chain_.orientation_end_body()).asAVector() - Utility::rotationMatrixToAxisAngle(ori_end_body_).asAVector();
		  std::cout << "oriRes: " << oriRes << std::endl;

		  for(int i = 0; i < 3; i++)
			  residuals[i + 3] = oriRes(i);

			if (jacobians != NULL && jacobians[0] != NULL)
			{
				mat66 jacob = chain_.jacobi_body(5, chain_.posi_end_last_last());
				std::cout << "yagjouni: " << jacob << std::endl;
				for(int i = 0; i < 6; i++)
					for(int j = 0; j < 6; j++)
						jacobians[0][i * 6 + j] = jacob(i, j);

			}
			return true;
	  }
	 private:
	  Chain& chain_;
	  vec3 posi_end_body_;
	  mat33 ori_end_body_;
	};

	double joints[size()];
	std::vector<double*> parameters;
	parameters.push_back(joints);
	int i = 0;
	for(iterator ii = begin(); ii < end(); ii++, i++)
		joints[i] = ii->theta().toFloat();

	Problem problem;
	CostFunction *costFunction = new IKCostFunction(*this, posi_end_body, ori_end_body);

	problem.AddResidualBlock(costFunction, NULL, parameters);

	Solver::Options options;
	options.minimizer_progress_to_stdout = true;
	options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
	options.jacobi_scaling = true;
	options.check_gradients = true;
	Solver::Summary summary;
	Solve(options, &problem, &summary);
	std::cout << summary.BriefReport() << "\n";

	for(unsigned i = 0; i < size(); i++)
		at(i).setTheta(parameters.at(0)[i]);

	return true;
}


/*
 * Copyright (c) 2011-2016, Graphics Lab, Georgia Tech Research Corporation
 * Copyright (c) 2011-2016, Humanoid Lab, Georgia Tech Research Corporation
 * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
 * All rights reserved.
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

// Algorithm details and publications: http://www.golems.org/node/1570

#include "dart/planning/Path.hpp"

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

namespace dart {
namespace planning {

class LinearPathSegment : public PathSegment
{
public:
	LinearPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &end) :
    PathSegment((end-start).norm()),
		start(start),
    end(end)
	{
	}

	Eigen::VectorXd getConfig(double s) const {
		s /= length;
		s = std::max(0.0, std::min(1.0, s));
		return (1.0 - s) * start + s * end;
	}

	Eigen::VectorXd getTangent(double /* s */) const {
		return (end - start) / length;
	}

	Eigen::VectorXd getCurvature(double /* s */) const {
		return Eigen::VectorXd::Zero(start.size());
	}

	list<double> getSwitchingPoints() const {
		return list<double>();
	}

	LinearPathSegment* clone() const {
		return new LinearPathSegment(*this);
	}

private:
	Eigen::VectorXd start;
	Eigen::VectorXd end;
};

class CircularPathSegment : public PathSegment
{
public:
	CircularPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &intersection, const Eigen::VectorXd &end, double maxDeviation) {
		if((intersection - start).norm() < 0.000001 || (end - intersection).norm() < 0.000001) {
			length = 0.0;
			radius = 1.0;
			center = intersection;
			x = Eigen::VectorXd::Zero(start.size());
			y = Eigen::VectorXd::Zero(start.size());
			return;
		}

		const Eigen::VectorXd startDirection = (intersection - start).normalized();
		const Eigen::VectorXd endDirection = (end - intersection).normalized();

		if((startDirection - endDirection).norm() < 0.000001) {
			length = 0.0;
			radius = 1.0;
			center = intersection;
			x = Eigen::VectorXd::Zero(start.size());
			y = Eigen::VectorXd::Zero(start.size());
			return;
		}

    // const double startDistance = (start - intersection).norm();
    // const double endDistance = (end - intersection).norm();

		double distance = std::min((start - intersection).norm(), (end - intersection).norm());
		const double angle = acos(startDirection.dot(endDirection));

		distance = std::min(distance, maxDeviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle)));  // enforce max deviation

		radius = distance / tan(0.5 * angle);
		length = angle * radius;

		center = intersection + (endDirection - startDirection).normalized() * radius / cos(0.5 * angle);
		x = (intersection - distance * startDirection - center).normalized();
		y = startDirection;

		//debug
		double dotStart = startDirection.dot((intersection - getConfig(0.0)).normalized());
		double dotEnd = endDirection.dot((getConfig(length) - intersection).normalized());
    if(std::abs(dotStart - 1.0) > 0.0001 || std::abs(dotEnd - 1.0) > 0.0001) {
			std::cout << "Error\n";
		}
	}

	Eigen::VectorXd getConfig(double s) const {
		const double angle = s / radius;
		return center + radius * (x * cos(angle) + y * sin(angle));
	}

	Eigen::VectorXd getTangent(double s) const {
		const double angle = s / radius;
		return - x * sin(angle) + y * cos(angle);
	}

	Eigen::VectorXd getCurvature(double s) const {
		const double angle = s / radius;
		return - 1.0 / radius * (x * cos(angle) + y * sin(angle));
	}

	list<double> getSwitchingPoints() const {
		list<double> switchingPoints;
		const double dim = x.size();
		for(unsigned int i = 0; i < dim; i++) {
			double switchingAngle = atan2(y[i], x[i]);
			if(switchingAngle < 0.0) {
				switchingAngle += M_PI;
			}
			const double switchingPoint = switchingAngle * radius;
			if(switchingPoint < length) {
				switchingPoints.push_back(switchingPoint);
			}
		}
		switchingPoints.sort();
		return switchingPoints;
	}

	CircularPathSegment* clone() const {
		return new CircularPathSegment(*this);
	}

private:
	double radius;
	Eigen::VectorXd center;
	Eigen::VectorXd x;
	Eigen::VectorXd y;
};

Path::Path(const list<VectorXd> &path, double maxDeviation) :
	length(0.0)
{
	if(path.size() < 2)
		return;
	list<VectorXd>::const_iterator config1 = path.begin();
	list<VectorXd>::const_iterator config2 = config1;
  ++config2;
	list<VectorXd>::const_iterator config3;
	VectorXd startConfig = *config1;
	while(config2 != path.end()) {
		config3 = config2;
    ++config3;
		if(maxDeviation > 0.0 && config3 != path.end()) {
			CircularPathSegment* blendSegment = new CircularPathSegment(0.5 * (*config1 + *config2), *config2, 0.5 * (*config2 + *config3), maxDeviation);
			VectorXd endConfig = blendSegment->getConfig(0.0);
			if((endConfig - startConfig).norm() > 0.000001) {
				pathSegments.push_back(new LinearPathSegment(startConfig, endConfig));
			}
			pathSegments.push_back(blendSegment);

			startConfig = blendSegment->getConfig(blendSegment->getLength());

			//debug
			if(((endConfig - *config1).norm() > 0.000001 && (*config2 - endConfig).norm() > 0.000001
        && std::abs((endConfig - *config1).normalized().dot((*config2 - endConfig).normalized()) - 1.0) > 0.000001)
				|| ((startConfig - *config2).norm() > 0.000001 && (*config3 - startConfig).norm() > 0.000001
        && std::abs((startConfig - *config2).normalized().dot((*config3 - startConfig).normalized()) - 1.0) > 0.000001)) {
					cout << "error" << endl;
			}
		}
		else {
			pathSegments.push_back(new LinearPathSegment(startConfig, *config2));
			startConfig = *config2;
		}
		config1 = config2;
    ++config2;
	}

	// create list of switching point candidates, calculate total path length and absolute positions of path segments
  for(list<PathSegment*>::iterator segment = pathSegments.begin(); segment != pathSegments.end(); ++segment) {
		(*segment)->position = length;
		list<double> localSwitchingPoints = (*segment)->getSwitchingPoints();
    for(list<double>::const_iterator point = localSwitchingPoints.begin(); point != localSwitchingPoints.end(); ++point) {
			switchingPoints.push_back(make_pair(length + *point, false));
		}
		length += (*segment)->getLength();
		switchingPoints.push_back(make_pair(length, true));
	}
	switchingPoints.pop_back();
}

Path::Path(const Path &path) :
	length(path.length),
	switchingPoints(path.switchingPoints)
{
  for(list<PathSegment*>::const_iterator it = path.pathSegments.begin(); it != path.pathSegments.end(); ++it) {
		pathSegments.push_back((*it)->clone());
	}
}

Path::~Path() {
  for(list<PathSegment*>::iterator it = pathSegments.begin(); it != pathSegments.end(); ++it) {
		delete *it;
	}
}

double Path::getLength() const {
	return length;
}

PathSegment* Path::getPathSegment(double &s) const {
	list<PathSegment*>::const_iterator it = pathSegments.begin();
	list<PathSegment*>::const_iterator next = it;
  ++next;
	while(next != pathSegments.end() && s >= (*next)->position) {
		it = next;
    ++next;
	}
	s -= (*it)->position;
	return *it;
}

VectorXd Path::getConfig(double s) const {
	const PathSegment* pathSegment = getPathSegment(s);
	return pathSegment->getConfig(s);
}

VectorXd Path::getTangent(double s) const {
	const PathSegment* pathSegment = getPathSegment(s);
	return pathSegment->getTangent(s);
}

VectorXd Path::getCurvature(double s) const {
	const PathSegment* pathSegment = getPathSegment(s);
	return pathSegment->getCurvature(s);
}

double Path::getNextSwitchingPoint(double s, bool &discontinuity) const {
	list<pair<double, bool> >::const_iterator it = switchingPoints.begin();
	while(it != switchingPoints.end() && it->first <= s) {
    ++it;
	}
	if(it == switchingPoints.end()) {
		discontinuity = true;
		return length;
	}
	else {
		discontinuity = it->second;
		return it->first;
	}
}

list<pair<double, bool> > Path::getSwitchingPoints() const {
	return switchingPoints;
}

} // namespace planning
} // namespace dart


#include <boost/program_options.hpp>
#include <chrono>
#include <cmath>
#include <map>
#include <random>
#include <set>

#include "../tools/util.hpp"
#include "bases.hpp"
#include "bilinear_form.hpp"

int bsd_rnd() {
  static unsigned int seed = 0;
  int a = 1103515245;
  int c = 12345;
  unsigned int m = 2147483648;
  return (seed = (a * seed + c) % m);
}

using namespace Time;
using namespace datastructures;

namespace po = boost::program_options;

template <typename I>
long long MaxIndex(int level);

template <>
long long MaxIndex<OrthonormalWaveletFn>(int level) {
  return std::pow(2, level);
}

template <>
long long MaxIndex<ThreePointWaveletFn>(int level) {
  return std::pow(2, level - 1);
}

template <typename I>
TreeVector<I> GradedTree(I *meta_root, int max_lvl, bool towards_origin,
                         double theta = 0.5) {
  TreeVector<I> result(meta_root);
  result.DeepRefine(/* call_filter */
                    [&](auto psi) {
                      if (towards_origin)
                        return (psi->level() <= max_lvl &&
                                psi->index() <=
                                    std::pow(1 + theta, psi->level()));
                      else
                        return (psi->level() <= max_lvl &&
                                psi->index() >=
                                    MaxIndex<I>(psi->level()) -
                                        std::pow(1 + theta, psi->level()));
                    }, /* call_postprocess */
                    [&](auto nv) {
                      nv->set_random();
                      nv->node()->Refine();
                    });
  return result;
}
auto Now() { return std::chrono::high_resolution_clock::now(); }
double Duration(std::chrono::high_resolution_clock::time_point start) {
  return std::chrono::duration<double>(Now() - start).count();
}

template <template <typename, typename> class Operator, typename WaveletBasisIn,
          typename WaveletBasisOut>
void TimeBilForm(std::string name,
                 const datastructures::TreeVector<WaveletBasisIn> &vec_in,
                 const datastructures::TreeVector<WaveletBasisOut> &vec_out) {
  double time_create = 0, time_apply = 0, time_apply_upp = 0,
         time_apply_low = 0;
  size_t iters = 0;
  while (time_apply < 20 && iters < 30) {
    ++iters;
    auto time_start = Now();
    auto bilform = CreateBilinearForm<Operator>(vec_in, vec_out);
    time_create += Duration(time_start);

    time_start = Now();
    bilform.Apply();
    time_apply += Duration(time_start);

    time_start = Now();
    bilform.ApplyUpp();
    time_apply_upp += Duration(time_start);

    time_start = Now();
    bilform.ApplyLow();
    time_apply_low += Duration(time_start);
  }
  std::cout << "\n\ttime-" << name << "-create: " << time_create / iters;
  std::cout << "\n\ttime-" << name << "-apply: " << time_apply / iters;
  std::cout << "\n\ttime-" << name << "-apply-upp: " << time_apply_upp / iters;
  std::cout << "\n\ttime-" << name << "-apply-low: " << time_apply_low / iters;
  std::cout << std::flush;
}

int main(int argc, char *argv[]) {
  double theta = 0.3;
  bool print_mesh = false;
  size_t max_iter = 999;
  boost::program_options::options_description adapt_optdesc("Refine options");
  adapt_optdesc.add_options()("theta", po::value<double>(&theta))(
      "print_mesh", po::value<bool>(&print_mesh))("max_iter",
                                                  po::value<size_t>(&max_iter));
  boost::program_options::options_description cmdline_options;
  cmdline_options.add(adapt_optdesc);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(),
            vm);
  po::notify(vm);
  std::cout << "Adaptive options:" << std::endl;
  std::cout << "\ttheta: " << theta << std::endl << std::endl;
  std::cout << std::endl;

  Bases B;
  int iter = 0;
  bool uniform_refine = theta >= 1;
  while (iter < max_iter) {
    std::cout << "iter: " << ++iter;

    if (uniform_refine) {
      TreeVector<OrthonormalWaveletFn> ortho_tree(B.ortho_tree.meta_root());
      TreeVector<ThreePointWaveletFn> threept_tree(
          B.three_point_tree.meta_root());
      ortho_tree.UniformRefine(std::array{iter}, /* grow_tree */ true);
      threept_tree.UniformRefine(std::array{iter}, /* grow_tree */ true);
      std::cout << "\n\tortho-tree-size: " << ortho_tree.Bfs().size()
                << "\n\tthreept-tree-size: " << threept_tree.Bfs().size()
                << "\n\ttotal-memory-kB: " << getmem() << std::flush;
      TimeBilForm<MassOperator>("M-o-o", ortho_tree, ortho_tree);
      TimeBilForm<MassOperator>("M-t-o", threept_tree, ortho_tree);
      TimeBilForm<TransportOperator>("T-t-o", threept_tree, ortho_tree);
    } else {
      // Local refine.
      auto ortho_tree_0 =
          GradedTree(B.ortho_tree.meta_root(), iter, true, theta);
      auto ortho_tree_1 =
          GradedTree(B.ortho_tree.meta_root(), iter, false, theta);
      auto threept_tree_0 =
          GradedTree(B.three_point_tree.meta_root(), iter, true, theta);
      auto threept_tree_1 =
          GradedTree(B.three_point_tree.meta_root(), iter, false, theta);
      std::cout << "\n\tortho-tree-0-size: " << ortho_tree_0.Bfs().size()
                << "\n\tortho-tree-1-size: " << ortho_tree_1.Bfs().size()
                << "\n\tthreept-tree-0-size: " << threept_tree_0.Bfs().size()
                << "\n\tthreept-tree-1-size: " << threept_tree_1.Bfs().size()
                << "\n\ttotal-memory-kB: " << getmem() << std::flush;

      TimeBilForm<MassOperator>("M-o0-o0", ortho_tree_0, ortho_tree_0);
      // TimeBilForm<MassOperator>("M-o0-o1", ortho_tree_0, ortho_tree_1);
      // TimeBilForm<MassOperator>("M-t0-t0", threept_tree_0, threept_tree_0);
      // TimeBilForm<MassOperator>("M-t0-t1", threept_tree_0, threept_tree_1);
      // TimeBilForm<MassOperator>("M-o0-t0", ortho_tree_0, threept_tree_0);
      // TimeBilForm<MassOperator>("M-o0-t1", ortho_tree_0, threept_tree_1);
      TimeBilForm<MassOperator>("M-t0-o0", threept_tree_0, ortho_tree_0);
      TimeBilForm<MassOperator>("M-t0-o1", threept_tree_0, ortho_tree_1);

      TimeBilForm<TransportOperator>("T-t0-o0", threept_tree_0, ortho_tree_0);
      TimeBilForm<TransportOperator>("T-t0-o1", threept_tree_0, ortho_tree_1);
      // TimeBilForm<TransportOperator>("T-t1-o0", threept_tree_1,
      // ortho_tree_0);

      if (print_mesh) {
        std::cout << "\n\tortho-tree-0: [";
        for (auto nv : ortho_tree_0.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
        std::cout << "\n\tortho-tree-1: [";
        for (auto nv : ortho_tree_1.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
        std::cout << "\n\tthreept-tree-0: [";
        for (auto nv : threept_tree_0.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
        std::cout << "\n\tthreept-tree-1: [";
        for (auto nv : threept_tree_1.Bfs())
          std::cout << "(" << nv->node()->level() << "," << nv->node()->center()
                    << "),";
        std::cout << "]" << std::flush;
      }
    }
    std::cout << std::endl;
  }
  return 0;
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program 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 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_planar_embedding
{
    template <class EdgeMap>
    class edge_inserter
    {
    public:
        edge_inserter(EdgeMap edge_map): _edge_map(edge_map) {}

        edge_inserter& operator++() { return *this; }
        edge_inserter& operator++(int) { return *this; }
        edge_inserter& operator*() { return *this; }

        template <class Key>
        edge_inserter& operator=(const Key& e)
        {
            _edge_map[e] = 1;
            return *this;
        }

    private:
        EdgeMap _edge_map;
    };

    template <class Graph, class VertexIndex, class EdgeIndex, class EmbedMap,
              class KurMap>
    void operator()(Graph& g, VertexIndex vertex_index, EdgeIndex edge_index,
                    EmbedMap embed_map, KurMap kur_map, bool& is_planar) const
    {
        edge_inserter<KurMap> kur_insert(kur_map);
        unchecked_vector_property_map
            <vector<typename graph_traits<Graph>::edge_descriptor>, VertexIndex>
            embedding(vertex_index, num_vertices(g));
        is_planar = boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::embedding = embedding,
             boyer_myrvold_params::kuratowski_subgraph = kur_insert);

        parallel_vertex_loop
            (g,
             [&](auto v)
             {
                 embed_map[v].clear();
                 for (auto& e : embedding[v])
                     embed_map[v].push_back(edge_index[e]);
             });
    }

    template <class Graph, class VertexIndex, class EdgeIndex, class KurMap>
    void operator()(Graph& g, VertexIndex, EdgeIndex edge_index,
                    dummy_property_map, KurMap kur_map,
                    bool& is_planar) const
    {
        edge_inserter<KurMap> kur_insert(kur_map);
        is_planar = boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::kuratowski_subgraph = kur_insert);
    }
};

bool is_planar(GraphInterface& gi, boost::any embed_map, boost::any kur_map)
{
    bool is_planar;

    if (embed_map.empty())
        embed_map = dummy_property_map();
    if (kur_map.empty())
        kur_map = dummy_property_map();

    typedef mpl::push_back<writable_edge_scalar_properties,
                           dummy_property_map>::type edge_map_types;
    typedef mpl::push_back<vertex_scalar_vector_properties,
                           dummy_property_map>::type vertex_map_types;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_planar_embedding(), std::placeholders::_1, gi.get_vertex_index(),
                       gi.get_edge_index(), std::placeholders::_2, std::placeholders::_3,
                       std::ref(is_planar)),
         vertex_map_types(), edge_map_types())
        (embed_map, kur_map);
    return is_planar;
}


#pragma once
#include <boost/function.hpp>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
/*
 * Numerical derivatives
 */

namespace sco {
using boost::function;
using Eigen::VectorXd;
using Eigen::MatrixXd;
class ScalarOfVector;
class VectorOfVector;
class MatrixOfVector;
typedef boost::shared_ptr<ScalarOfVector> ScalarOfVectorPtr;
typedef boost::shared_ptr<VectorOfVector> VectorOfVectorPtr;
typedef boost::shared_ptr<MatrixOfVector> MatrixOfVectorPtr;

class ScalarOfVector {
public:
  virtual double operator()(const VectorXd& x) const = 0;
  double call(const VectorXd& x) const {return operator()(x);}
  virtual ~ScalarOfVector() {}

  typedef function<double(VectorXd)> boost_func;
  static ScalarOfVectorPtr construct(const boost_func&);
  //  typedef VectorXd (*c_func)(const VectorXd&);
  //  static ScalarOfVectorPtr construct(const c_func&);

};
class VectorOfVector {
public:
  virtual VectorXd operator()(const VectorXd& x) const = 0;
  VectorXd call(const VectorXd& x) const {return operator()(x);}
  virtual ~VectorOfVector() {}

  typedef function<VectorXd(VectorXd)> boost_func;
  static VectorOfVectorPtr construct(const boost_func&);
  //  typedef VectorXd (*c_func)(const VectorXd&);
  //  static VectorOfVectorPtr construct(const c_func&);

};
class MatrixOfVector {
public:
  virtual MatrixXd operator()(const VectorXd& x) const = 0;
  MatrixXd call(const VectorXd& x) const {return operator()(x);}
  virtual ~MatrixOfVector() {}

  typedef function<MatrixXd(VectorXd)> boost_func;
  static MatrixOfVectorPtr construct(const boost_func&);
  //  typedef VectorMatrixXd (*c_func)(const VectorXd&);
  //  static MatrixOfVectorPtr construct(const c_func&);
};

VectorXd calcForwardNumGrad(const ScalarOfVector& f, const VectorXd& x, double epsilon);
MatrixXd calcForwardNumJac(const VectorOfVector& f, const VectorXd& x, double epsilon);
void calcGradAndDiagHess(const ScalarOfVector& f, const VectorXd& x, double epsilon,
    double& y, VectorXd& grad, VectorXd& hess);
void calcGradHess(ScalarOfVectorPtr f, const VectorXd& x, double epsilon,
    double& y, VectorXd& grad, MatrixXd& hess);
VectorOfVectorPtr forwardNumGrad(ScalarOfVectorPtr f, double epsilon);
MatrixOfVectorPtr forwardNumJac(VectorOfVectorPtr f, double epsilon);

}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP
#define BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP

#ifdef _MSC_VER
#pragma once
#endif
#include <boost/multiprecision/detail/number_base.hpp> // test for multiprecision types.
#include <boost/type_traits/is_complex.hpp> // test for complex types

#include <iostream>
#include <utility>
#include <boost/config/no_tr1/cmath.hpp>
#include <stdexcept>

#include <boost/math/tools/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/assert.hpp>
#include <boost/throw_exception.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4512)
#endif
#include <boost/math/tools/tuple.hpp>
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/math/special_functions/sign.hpp>
#include <boost/math/tools/toms748_solve.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{ namespace tools{

namespace detail{

namespace dummy{

   template<int n, class T>
   typename T::value_type get(const T&) BOOST_MATH_NOEXCEPT(T);
}

template <class Tuple, class T>
void unpack_tuple(const Tuple& t, T& a, T& b) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Use ADL to find the right overload for get:
   a = get<0>(t);
   b = get<1>(t);
}
template <class Tuple, class T>
void unpack_tuple(const Tuple& t, T& a, T& b, T& c) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Use ADL to find the right overload for get:
   a = get<0>(t);
   b = get<1>(t);
   c = get<2>(t);
}

template <class Tuple, class T>
inline void unpack_0(const Tuple& t, T& val) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Rely on ADL to find the correct overload of get:
   val = get<0>(t);
}

template <class T, class U, class V>
inline void unpack_tuple(const std::pair<T, U>& p, V& a, V& b) BOOST_MATH_NOEXCEPT(T)
{
   a = p.first;
   b = p.second;
}
template <class T, class U, class V>
inline void unpack_0(const std::pair<T, U>& p, V& a) BOOST_MATH_NOEXCEPT(T)
{
   a = p.first;
}

template <class F, class T>
void handle_zero_derivative(F f,
                            T& last_f0,
                            const T& f0,
                            T& delta,
                            T& result,
                            T& guess,
                            const T& min,
                            const T& max) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   if(last_f0 == 0)
   {
      // this must be the first iteration, pretend that we had a
      // previous one at either min or max:
      if(result == min)
      {
         guess = max;
      }
      else
      {
         guess = min;
      }
      unpack_0(f(guess), last_f0);
      delta = guess - result;
   }
   if(sign(last_f0) * sign(f0) < 0)
   {
      // we've crossed over so move in opposite direction to last step:
      if(delta < 0)
      {
         delta = (result - min) / 2;
      }
      else
      {
         delta = (result - max) / 2;
      }
   }
   else
   {
      // move in same direction as last step:
      if(delta < 0)
      {
         delta = (result - max) / 2;
      }
      else
      {
         delta = (result - min) / 2;
      }
   }
}

} // namespace

template <class F, class T, class Tol, class Policy>
std::pair<T, T> bisect(F f, T min, T max, Tol tol, boost::uintmax_t& max_iter, const Policy& pol) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<Policy>::value && BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   T fmin = f(min);
   T fmax = f(max);
   if(fmin == 0)
   {
      max_iter = 2;
      return std::make_pair(min, min);
   }
   if(fmax == 0)
   {
      max_iter = 2;
      return std::make_pair(max, max);
   }

   //
   // Error checking:
   //
   static const char* function = "boost::math::tools::bisect<%1%>";
   if(min >= max)
   {
      return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function,
         "Arguments in wrong order in boost::math::tools::bisect (first arg=%1%)", min, pol));
   }
   if(fmin * fmax >= 0)
   {
      return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function,
         "No change of sign in boost::math::tools::bisect, either there is no root to find, or there are multiple roots in the interval (f(min) = %1%).", fmin, pol));
   }

   //
   // Three function invocations so far:
   //
   boost::uintmax_t count = max_iter;
   if(count < 3)
      count = 0;
   else
      count -= 3;

   while(count && (0 == tol(min, max)))
   {
      T mid = (min + max) / 2;
      T fmid = f(mid);
      if((mid == max) || (mid == min))
         break;
      if(fmid == 0)
      {
         min = max = mid;
         break;
      }
      else if(sign(fmid) * sign(fmin) < 0)
      {
         max = mid;
         fmax = fmid;
      }
      else
      {
         min = mid;
         fmin = fmid;
      }
      --count;
   }

   max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
   std::cout << "Bisection iteration, final count = " << max_iter << std::endl;

   static boost::uintmax_t max_count = 0;
   if(max_iter > max_count)
   {
      max_count = max_iter;
      std::cout << "Maximum iterations: " << max_iter << std::endl;
   }
#endif

   return std::make_pair(min, max);
}

template <class F, class T, class Tol>
inline std::pair<T, T> bisect(F f, T min, T max, Tol tol, boost::uintmax_t& max_iter)  BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value && BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return bisect(f, min, max, tol, max_iter, policies::policy<>());
}

template <class F, class T, class Tol>
inline std::pair<T, T> bisect(F f, T min, T max, Tol tol) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value && BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return bisect(f, min, max, tol, m, policies::policy<>());
}

template <class F, class T>
T newton_raphson_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   BOOST_MATH_STD_USING

   T f0(0), f1, last_f0(0);
   T result = guess;

   T factor = static_cast<T>(ldexp(1.0, 1 - digits));
   T delta = tools::max_value<T>();
   T delta1 = tools::max_value<T>();
   T delta2 = tools::max_value<T>();

   boost::uintmax_t count(max_iter);

   do{
      last_f0 = f0;
      delta2 = delta1;
      delta1 = delta;
      detail::unpack_tuple(f(result), f0, f1);
      --count;
      if(0 == f0)
         break;
      if(f1 == 0)
      {
         // Oops zero derivative!!!
#ifdef BOOST_MATH_INSTRUMENT
         std::cout << "Newton iteration, zero derivative found" << std::endl;
#endif
         detail::handle_zero_derivative(f, last_f0, f0, delta, result, guess, min, max);
      }
      else
      {
         delta = f0 / f1;
      }
#ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Newton iteration, delta = " << delta << std::endl;
#endif
      if(fabs(delta * 2) > fabs(delta2))
      {
         // last two steps haven't converged.
         T shift = (delta > 0) ? (result - min) / 2 : (result - max) / 2;
         if ((result != 0) && (fabs(shift) > fabs(result)))
         {
            delta = sign(delta) * result; // protect against huge jumps!
         }
         else
            delta = shift;
         // reset delta1/2 so we don't take this branch next time round:
         delta1 = 3 * delta;
         delta2 = 3 * delta;
      }
      guess = result;
      result -= delta;
      if(result <= min)
      {
         delta = 0.5F * (guess - min);
         result = guess - delta;
         if((result == min) || (result == max))
            break;
      }
      else if(result >= max)
      {
         delta = 0.5F * (guess - max);
         result = guess - delta;
         if((result == min) || (result == max))
            break;
      }
      // update brackets:
      if(delta > 0)
         max = guess;
      else
         min = guess;
   }while(count && (fabs(result * factor) < fabs(delta)));

   max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
   std::cout << "Newton Raphson iteration, final count = " << max_iter << std::endl;

   static boost::uintmax_t max_count = 0;
   if(max_iter > max_count)
   {
      max_count = max_iter;
      std::cout << "Maximum iterations: " << max_iter << std::endl;
   }
#endif

   return result;
}

template <class F, class T>
inline T newton_raphson_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return newton_raphson_iterate(f, guess, min, max, digits, m);
}

namespace detail{

   struct halley_step
   {
      template <class T>
      static T step(const T& /*x*/, const T& f0, const T& f1, const T& f2) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T))
      {
         using std::fabs;
         T denom = 2 * f0;
         T num = 2 * f1 - f0 * (f2 / f1);
         T delta;

         BOOST_MATH_INSTRUMENT_VARIABLE(denom);
         BOOST_MATH_INSTRUMENT_VARIABLE(num);

         if((fabs(num) < 1) && (fabs(denom) >= fabs(num) * tools::max_value<T>()))
         {
            // possible overflow, use Newton step:
            delta = f0 / f1;
         }
         else
            delta = denom / num;
         return delta;
      }
   };

   template <class Stepper, class F, class T>
   T second_order_root_finder(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
   {
      BOOST_MATH_STD_USING

         T f0(0), f1, f2;
      T result = guess;

      T factor = ldexp(static_cast<T>(1.0), 1 - digits);
      T delta = (std::max)(T(10000000 * guess), T(10000000));  // arbitarily large delta
      T last_f0 = 0;
      T delta1 = delta;
      T delta2 = delta;
      bool out_of_bounds_sentry = false;

#ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Second order root iteration, limit = " << factor << std::endl;
#endif

      boost::uintmax_t count(max_iter);

      do{
         last_f0 = f0;
         delta2 = delta1;
         delta1 = delta;
         detail::unpack_tuple(f(result), f0, f1, f2);
         --count;

         BOOST_MATH_INSTRUMENT_VARIABLE(f0);
         BOOST_MATH_INSTRUMENT_VARIABLE(f1);
         BOOST_MATH_INSTRUMENT_VARIABLE(f2);

         if(0 == f0)
            break;
         if(f1 == 0)
         {
            // Oops zero derivative!!!
#ifdef BOOST_MATH_INSTRUMENT
            std::cout << "Second order root iteration, zero derivative found" << std::endl;
#endif
            detail::handle_zero_derivative(f, last_f0, f0, delta, result, guess, min, max);
         }
         else
         {
            if(f2 != 0)
            {
               delta = Stepper::step(result, f0, f1, f2);
               if(delta * f1 / f0 < 0)
               {
                  // Oh dear, we have a problem as Newton and Halley steps
                  // disagree about which way we should move.  Probably
                  // there is cancelation error in the calculation of the
                  // Halley step, or else the derivatives are so small
                  // that their values are basically trash.  We will move
                  // in the direction indicated by a Newton step, but
                  // by no more than twice the current guess value, otherwise
                  // we can jump way out of bounds if we're not careful.
                  // See https://svn.boost.org/trac/boost/ticket/8314.
                  delta = f0 / f1;
                  if(fabs(delta) > 2 * fabs(guess))
                     delta = (delta < 0 ? -1 : 1) * 2 * fabs(guess);
               }
            }
            else
               delta = f0 / f1;
         }
#ifdef BOOST_MATH_INSTRUMENT
         std::cout << "Second order root iteration, delta = " << delta << std::endl;
#endif
         T convergence = fabs(delta / delta2);
         if((convergence > 0.8) && (convergence < 2))
         {
            // last two steps haven't converged.
            delta = (delta > 0) ? (result - min) / 2 : (result - max) / 2;
            if ((result != 0) && (fabs(delta) > result))
               delta = sign(delta) * result; // protect against huge jumps!
            // reset delta2 so that this branch will *not* be taken on the
            // next iteration:
            delta2 = delta * 3;
            delta1 = delta * 3;
            BOOST_MATH_INSTRUMENT_VARIABLE(delta);
         }
         guess = result;
         result -= delta;
         BOOST_MATH_INSTRUMENT_VARIABLE(result);

         // check for out of bounds step:
         if(result < min)
         {
            T diff = ((fabs(min) < 1) && (fabs(result) > 1) && (tools::max_value<T>() / fabs(result) < fabs(min)))
               ? T(1000)
               : (fabs(min) < 1) && (fabs(tools::max_value<T>() * min) < fabs(result))
               ? ((min < 0) != (result < 0)) ? -tools::max_value<T>() : tools::max_value<T>() : T(result / min);
            if(fabs(diff) < 1)
               diff = 1 / diff;
            if(!out_of_bounds_sentry && (diff > 0) && (diff < 3))
            {
               // Only a small out of bounds step, lets assume that the result
               // is probably approximately at min:
               delta = 0.99f * (guess - min);
               result = guess - delta;
               out_of_bounds_sentry = true; // only take this branch once!
            }
            else
            {
               delta = (guess - min) / 2;
               result = guess - delta;
               if((result == min) || (result == max))
                  break;
            }
         }
         else if(result > max)
         {
            T diff = ((fabs(max) < 1) && (fabs(result) > 1) && (tools::max_value<T>() / fabs(result) < fabs(max))) ? T(1000) : T(result / max);
            if(fabs(diff) < 1)
               diff = 1 / diff;
            if(!out_of_bounds_sentry && (diff > 0) && (diff < 3))
            {
               // Only a small out of bounds step, lets assume that the result
               // is probably approximately at min:
               delta = 0.99f * (guess - max);
               result = guess - delta;
               out_of_bounds_sentry = true; // only take this branch once!
            }
            else
            {
               delta = (guess - max) / 2;
               result = guess - delta;
               if((result == min) || (result == max))
                  break;
            }
         }
         // update brackets:
         if(delta > 0)
            max = guess;
         else
            min = guess;
      } while(count && (fabs(result * factor) < fabs(delta)));

      max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Second order root iteration, final count = " << max_iter << std::endl;
#endif

      return result;
   }

}

template <class F, class T>
T halley_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::halley_step>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T halley_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return halley_iterate(f, guess, min, max, digits, m);
}

namespace detail{

   struct schroder_stepper
   {
      template <class T>
      static T step(const T& x, const T& f0, const T& f1, const T& f2) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T))
      {
         using std::fabs;
         T ratio = f0 / f1;
         T delta;
         if((x != 0) && (fabs(ratio / x) < 0.1))
         {
            delta = ratio + (f2 / (2 * f1)) * ratio * ratio;
            // check second derivative doesn't over compensate:
            if(delta * ratio < 0)
               delta = ratio;
         }
         else
            delta = ratio;  // fall back to Newton iteration.
         return delta;
      }
   };

}

template <class F, class T>
T schroder_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::schroder_stepper>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T schroder_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return schroder_iterate(f, guess, min, max, digits, m);
}
//
// These two are the old spelling of this function, retained for backwards compatibity just in case:
//
template <class F, class T>
T schroeder_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::schroder_stepper>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T schroeder_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return schroder_iterate(f, guess, min, max, digits, m);
}

#ifndef BOOST_NO_CXX11_AUTO_DECLARATIONS
/*
 * Why do we set the default maximum number of iterations to the number of digits in the type?
 * Because for double roots, the number of digits increases linearly with the number of iterations,
 * so this default should recover full precision even in this somewhat pathological case.
 * For isolated roots, the problem is so rapidly convergent that this doesn't matter at all.
 */
template<class Complex, class F>
Complex complex_newton(F g, Complex guess, int max_iterations=std::numeric_limits<typename Complex::value_type>::digits)
{
    typedef typename Complex::value_type Real;
    using std::norm;
    using std::abs;
    using std::max;
    // z0, z1, and z2 cannot be the same, in case we immediately need to resort to Muller's Method:
    Complex z0 = guess + Complex(1,0);
    Complex z1 = guess + Complex(0,1);
    Complex z2 = guess;

    do {
       auto pair = g(z2);
       if (norm(pair.second) == 0)
       {
           // Muller's method. Notation follows Numerical Recipes, 9.5.2:
           Complex q = (z2 - z1)/(z1 - z0);
           auto P0 = g(z0);
           auto P1 = g(z1);
           Complex qp1 = static_cast<Complex>(1)+q;
           Complex A = q*(pair.first - qp1*P1.first + q*P0.first);

           Complex B = (static_cast<Complex>(2)*q+static_cast<Complex>(1))*pair.first - qp1*qp1*P1.first +q*q*P0.first;
           Complex C = qp1*pair.first;
           Complex rad = sqrt(B*B - static_cast<Complex>(4)*A*C);
           Complex denom1 = B + rad;
           Complex denom2 = B - rad;
           Complex correction = (z1-z2)*static_cast<Complex>(2)*C;
           if (norm(denom1) > norm(denom2))
           {
               correction /= denom1;
           }
           else
           {
               correction /= denom2;
           }

           z0 = z1;
           z1 = z2;
           z2 = z2 + correction;
       }
       else
       {
           z0 = z1;
           z1 = z2;
           z2 = z2  - (pair.first/pair.second);
       }

       // See: https://math.stackexchange.com/questions/3017766/constructing-newton-iteration-converging-to-non-root
       // If f' is continuous, then convergence of x_n -> x* implies f(x*) = 0.
       // This condition approximates this convergence condition by requiring three consecutive iterates to be clustered.
       Real tol = max(abs(z2)*std::numeric_limits<Real>::epsilon(), std::numeric_limits<Real>::epsilon());
       bool real_close = abs(z0.real() - z1.real()) < tol && abs(z0.real() - z2.real()) < tol && abs(z1.real() - z2.real()) < tol;
       bool imag_close = abs(z0.imag() - z1.imag()) < tol && abs(z0.imag() - z2.imag()) < tol && abs(z1.imag() - z2.imag()) < tol;
       if (real_close && imag_close)
       {
           return z2;
       }

   } while(max_iterations--);

    // The idea is that if we can get abs(f) < eps, we should, but if we go through all these iterations
    // and abs(f) < sqrt(eps), then roundoff error simply does not allow that we can evaluate f to < eps
    // This is somewhat awkward as it isn't scale invariant, but using the Daubechies coefficient example code,
    // I found this condition generates correct roots, whereas the scale invariant condition discussed here:
    // https://scicomp.stackexchange.com/questions/30597/defining-a-condition-number-and-termination-criteria-for-newtons-method
    // allows nonroots to be passed off as roots.
    auto pair = g(z2);
    if (abs(pair.first) < sqrt(std::numeric_limits<Real>::epsilon()))
    {
        return z2;
    }

    return {std::numeric_limits<Real>::quiet_NaN(),
            std::numeric_limits<Real>::quiet_NaN()};
}
#endif

#if !defined(BOOST_NO_CXX17_IF_CONSTEXPR)
// https://stackoverflow.com/questions/48979861/numerically-stable-method-for-solving-quadratic-equations/50065711
namespace detail
{
    template<class T>
    inline T discriminant(T const & a, T const & b, T const & c)
    {
        T w = 4*a*c;
        T e = std::fma(-c, 4*a, w);
        T f = std::fma(b, b, -w);
        return f + e;
    }
}

template<class T>
auto quadratic_roots(T const& a, T const& b, T const& c)
{
    using std::copysign;
    using std::sqrt;
    if constexpr (std::is_integral<T>::value)
    {
        // What I want is to write:
        // return quadratic_roots(double(a), double(b), double(c));
        // but that doesn't compile.
        double nan = std::numeric_limits<double>::quiet_NaN();
        if(a==0)
        {
            if (b==0 && c != 0)
            {
                return std::pair<double, double>(nan, nan);
            }
            else if (b==0 && c==0)
            {
                return std::pair<double, double>(0,0);
            }
            return std::pair<double, double>(-c/b, -c/b);
        }
        if (b==0)
        {
            double x0_sq = -double(c)/double(a);
            if (x0_sq < 0) {
                return std::pair<double, double>(nan, nan);
            }
            double x0 = sqrt(x0_sq);
            return std::pair<double, double>(-x0,x0);
        }
        double discriminant = detail::discriminant(double(a), double(b), double(c));
        if (discriminant < 0)
        {
            return std::pair<double, double>(nan, nan);
        }
        double q = -(b + copysign(sqrt(discriminant), double(b)))/T(2);
        double x0 = q/a;
        double x1 = c/q;
        if (x0 < x1) {
            return std::pair<double, double>(x0, x1);
        }
        return std::pair<double, double>(x1, x0);
    }
    else if constexpr (std::is_floating_point<T>::value)
    {
        T nan = std::numeric_limits<T>::quiet_NaN();
        if(a==0)
        {
            if (b==0 && c != 0)
            {
                return std::pair<T, T>(nan, nan);
            }
            else if (b==0 && c==0)
            {
                return std::pair<T, T>(0,0);
            }
            return std::pair<T, T>(-c/b, -c/b);
        }
        if (b==0)
        {
            T x0_sq = -c/a;
            if (x0_sq < 0) {
                return std::pair<T, T>(nan, nan);
            }
            T x0 = sqrt(x0_sq);
            return std::pair<T, T>(-x0,x0);
        }
        T discriminant = detail::discriminant(a, b, c);
        // Is there a sane way to flush very small negative values to zero?
        // If there is I don't know of it.
        if (discriminant < 0)
        {
            return std::pair<T, T>(nan, nan);
        }
        T q = -(b + copysign(sqrt(discriminant), b))/T(2);
        T x0 = q/a;
        T x1 = c/q;
        if (x0 < x1)
        {
            return std::pair<T, T>(x0, x1);
        }
        return std::pair<T, T>(x1, x0);
    }
    else if constexpr (boost::is_complex<T>::value || boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_complex)
    {
        typename T::value_type nan = std::numeric_limits<typename T::value_type>::quiet_NaN();
        if(a.real()==0 && a.imag() ==0)
        {
            using std::norm;
            if (b.real()==0 && b.imag() && norm(c) != 0)
            {
                return std::pair<T, T>({nan, nan}, {nan, nan});
            }
            else if (b.real()==0 && b.imag() && c.real() ==0 && c.imag() == 0)
            {
                return std::pair<T, T>({0,0},{0,0});
            }
            return std::pair<T, T>(-c/b, -c/b);
        }
        if (b.real()==0 && b.imag() == 0)
        {
            T x0_sq = -c/a;
            T x0 = sqrt(x0_sq);
            return std::pair<T, T>(-x0, x0);
        }
        // There's no fma for complex types:
        T discriminant = b*b - T(4)*a*c;
        T q = -(b + sqrt(discriminant))/T(2);
        return std::pair<T, T>(q/a, c/q);
    }
    else // Most likely the type is a boost.multiprecision.
    {    //There is no fma for multiprecision, and in addition it doesn't seem to be useful, so revert to the naive computation.
        T nan = std::numeric_limits<T>::quiet_NaN();
        if(a==0)
        {
            if (b==0 && c != 0)
            {
                return std::pair<T, T>(nan, nan);
            }
            else if (b==0 && c==0)
            {
                return std::pair<T, T>(0,0);
            }
            return std::pair<T, T>(-c/b, -c/b);
        }
        if (b==0)
        {
            T x0_sq = -c/a;
            if (x0_sq < 0) {
                return std::pair<T, T>(nan, nan);
            }
            T x0 = sqrt(x0_sq);
            return std::pair<T, T>(-x0,x0);
        }
        T discriminant = b*b - 4*a*c;
        if (discriminant < 0)
        {
            return std::pair<T, T>(nan, nan);
        }
        T q = -(b + copysign(sqrt(discriminant), b))/T(2);
        T x0 = q/a;
        T x1 = c/q;
        if (x0 < x1)
        {
            return std::pair<T, T>(x0, x1);
        }
        return std::pair<T, T>(x1, x0);
    }
}
#endif

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP