#include <cstdlib>
#include <algorithm>
#include <complex>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>

#if __APPLE__
#include <Accelerate/Accelerate.h>
#endif

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include <QtCore/QVector>

#include "EbsdLib/Core/EbsdDataArray.hpp"
#include "EbsdLib/Core/OrientationTransformation.hpp"
#include "EbsdLib/Core/Quaternion.hpp"
#include "EbsdLib/EbsdLib.h"
#include "EbsdLib/Math/ArrayHelpers.hpp"
#include "EbsdLib/Math/EbsdLibMath.h"
#include "EbsdLib/Math/EbsdMatrixMath.h"
#include "EbsdLib/OrientationMath/OrientationConverter.hpp"

#include "TestPrintFunctions.h"
#include "UnitTestSupport.hpp"

#include "EbsdLib/Test/EbsdLibTestFileLocations.h"

using namespace EbsdLib::Constants;

class OrientationArrayTest
{
public:
  OrientationArrayTest() = default;
  virtual ~OrientationArrayTest() = default;

  OrientationArrayTest(const OrientationArrayTest&) = delete;            // Copy Constructor Not Implemented
  OrientationArrayTest(OrientationArrayTest&&) = delete;                 // Move Constructor Not Implemented
  OrientationArrayTest& operator=(const OrientationArrayTest&) = delete; // Copy Assignment Not Implemented
  OrientationArrayTest& operator=(OrientationArrayTest&&) = delete;      // Move Assignment Not Implemented

  using FOrientArrayType = Orientation<float>;
  using FloatVectorType = std::vector<float>;
  using FloatQVectorType = QVector<float>;
  using DoubleVectorType = std::vector<double>;
  using DoubleQVectorType = QVector<double>;

  EBSD_GET_NAME_OF_CLASS_DECL(OrientationArrayTest)

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T>
  T transfer_sign(T a, T b)
  {
    if(a > 0.0 && b > 0.0)
    {
      return a;
    }
    if(a < 0.0 && b > 0.0)
    {
      return -1 * a;
    }

    if(a < 0.0 && b < 0.0)
    {
      return a;
    }

    return -1 * a;
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void TestRotArray()
  {
    FOrientArrayType ro(3);
    ro[0] = 2;
    ro[1] = 4;
    ro[2] = 6;
    //  float sum = ro.sum();
    //  DREAM3D_REQUIRE_EQUAL(sum, 12);

    //  DREAM3D_REQUIRE_EQUAL(ro.size(), 3);

    //  DREAM3D_REQUIRE_EQUAL(ro.maxval(), 6);
    //  DREAM3D_REQUIRE_EQUAL(ro.minval(), 2);

    //  float pro = ro.product();
    //  DREAM3D_REQUIRE_EQUAL(pro, 48);
    //  sum = result.sum();
    //  float max = result.maxval();
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_eu_check()
  {
    {
      OrientationTransformation::ResultType result;
      using TestType = FOrientArrayType;
      FOrientArrayType eu_r(3);
      eu_r[0] = 0.81585413f;
      eu_r[1] = 3.00f;
      eu_r[2] = 0.8661895f;
      result = OrientationTransformation::eu_check<TestType>(eu_r);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      eu_r[1] = eu_r[1] - EbsdLib::Constants::k_Pi;
      result = OrientationTransformation::eu_check<TestType>(eu_r);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);
    }
    {
      using TestType = std::vector<float>;
      OrientationTransformation::ResultType result;
      FloatVectorType eu_v(3);
      eu_v[0] = 1.0f;
      eu_v[1] = 0.4f;
      eu_v[2] = 0.9f;
      result = OrientationTransformation::eu_check<TestType>(eu_v);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      eu_v[0] = -1.0;
      result = OrientationTransformation::eu_check<TestType>(eu_v);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }
    {
      using TestType = QVector<float>;
      OrientationTransformation::ResultType result;
      FloatQVectorType eu_q(3);
      eu_q[0] = 1.0f;
      eu_q[1] = 0.4f;
      eu_q[2] = 0.9f;
      result = OrientationTransformation::eu_check<TestType>(eu_q);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      eu_q[0] = -1.0;
      result = OrientationTransformation::eu_check<TestType>(eu_q);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_ro_check()
  {
    {
      OrientationTransformation::ResultType result;
      FOrientArrayType ro(4);
      using TestType = FOrientArrayType;

      ro[0] = 1.0f;
      ro[1] = 1.0f;
      ro[2] = 1.0f;
      ro[3] = 1.0f;
      EbsdMatrixMath::Normalize3x1(&(ro[0]));
      result = OrientationTransformation::ro_check<TestType>(ro);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      ro[3] = -1.0;
      result = OrientationTransformation::ro_check<TestType>(ro);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = std::vector<float>;
      OrientationTransformation::ResultType result;

      FloatVectorType ro(4);
      ro[0] = 1.0f;
      ro[1] = 1.0f;
      ro[2] = 1.0f;
      ro[3] = 1.0f;
      EbsdMatrixMath::Normalize3x1(&(ro[0]));
      result = OrientationTransformation::ro_check<TestType>(ro);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      ro[3] = -1.0;
      result = OrientationTransformation::ro_check<TestType>(ro);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = QVector<float>;
      OrientationTransformation::ResultType result;
      FloatQVectorType ro(4);
      ro[0] = 1.0f;
      ro[1] = 1.0f;
      ro[2] = 1.0f;
      ro[3] = 1.0f;
      EbsdMatrixMath::Normalize3x1(&(ro[0]));
      result = OrientationTransformation::ro_check<TestType>(ro);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      ro[3] = -1.0;
      result = OrientationTransformation::ro_check<TestType>(ro);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_ho_check()
  {
    {
      using TestType = FOrientArrayType;
      OrientationTransformation::ResultType result;
      FOrientArrayType ho(3);
      ho[0] = 0.5f;
      ho[1] = 0.5f;
      ho[2] = 0.5f;
      result = OrientationTransformation::ho_check<TestType>(ho);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      ho[2] = 8.0;
      result = OrientationTransformation::ho_check<TestType>(ho);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = std::vector<float>;
      OrientationTransformation::ResultType result;

      FloatVectorType ho(3);
      ho[0] = 0.5f;
      ho[1] = 0.5f;
      ho[2] = 0.5f;
      result = OrientationTransformation::ho_check<TestType>(ho);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      ho[2] = 8.0;
      result = OrientationTransformation::ho_check<TestType>(ho);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = QVector<float>;
      OrientationTransformation::ResultType result;
      FloatQVectorType ho(3);
      ho[0] = 0.5f;
      ho[1] = 0.5f;
      ho[2] = 0.5f;
      result = OrientationTransformation::ho_check<TestType>(ho);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      ho[2] = 8.0;
      result = OrientationTransformation::ho_check<TestType>(ho);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_cu_check()
  {
    {
      using TestType = FOrientArrayType;
      OrientationTransformation::ResultType result;
      FOrientArrayType v(3);
      v[0] = 0.5f;
      v[1] = 0.5f;
      v[2] = 0.5f;
      result = OrientationTransformation::cu_check<TestType>(v);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      v[2] = 8.0;
      result = OrientationTransformation::cu_check<TestType>(v);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = std::vector<float>;
      OrientationTransformation::ResultType result;
      FloatVectorType v(3);
      v[0] = 0.5f;
      v[1] = 0.5f;
      v[2] = 0.5f;
      result = OrientationTransformation::cu_check<TestType>(v);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      v[2] = 8.0;
      result = OrientationTransformation::cu_check<TestType>(v);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = QVector<float>;
      OrientationTransformation::ResultType result;
      FloatQVectorType v(3);
      v[0] = 0.5f;
      v[1] = 0.5f;
      v[2] = 0.5f;
      result = OrientationTransformation::cu_check<TestType>(v);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);
      v[2] = 8.0;
      result = OrientationTransformation::cu_check<TestType>(v);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_qu_check()
  {
    QuatF quat(1.0, 1.0, 1.0, 0.0);
    quat = quat.unitQuaternion();

    {
      using TestType = FOrientArrayType;
      OrientationTransformation::ResultType result;
      FOrientArrayType qu(4);
      qu[0] = quat.x();
      qu[1] = quat.y();
      qu[2] = quat.z();
      qu[3] = quat.w();
      EbsdMatrixMath::Normalize3x1(&(qu[0]));
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      qu[0] = 1.5f;
      qu[1] = 3.0f;
      qu[2] = 2.0f;
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);

      qu[0] = -1.0;
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);
    }

    {
      using TestType = std::vector<float>;
      OrientationTransformation::ResultType result;
      FloatVectorType qu(4);
      qu[0] = quat.x();
      qu[1] = quat.y();
      qu[2] = quat.z();
      qu[3] = quat.w();
      EbsdMatrixMath::Normalize3x1(&(qu[0]));
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      qu[0] = 1.5f;
      qu[1] = 3.0f;
      qu[2] = 2.0f;
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);

      qu[0] = -1.0;
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);
    }

    {
      using TestType = QVector<float>;
      OrientationTransformation::ResultType result;
      FloatQVectorType qu(4);
      qu[0] = quat.x();
      qu[1] = quat.y();
      qu[2] = quat.z();
      qu[3] = quat.w();
      EbsdMatrixMath::Normalize3x1(&(qu[0]));
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      qu[0] = 1.5f;
      qu[1] = 3.0f;
      qu[2] = 2.0f;
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);

      qu[0] = -1.0;
      result = OrientationTransformation::qu_check<TestType>(qu);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_ax_check()
  {
    {
      using TestType = FOrientArrayType;
      OrientationTransformation::ResultType result;
      FOrientArrayType ax(4);
      ax[0] = 0.0f;
      ax[1] = 0.0f;
      ax[2] = 1.0f;
      ax[3] = EbsdLib::Constants::k_Pi - 0.00001f;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      ax[0] = 1.0;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);

      ax[3] = EbsdLib::Constants::k_Pi + 1.0f;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = std::vector<float>;
      OrientationTransformation::ResultType result;
      FloatVectorType ax(4);
      ax[0] = 0.0f;
      ax[1] = 0.0f;
      ax[2] = 1.0f;
      ax[3] = EbsdLib::Constants::k_Pi - 0.00001f;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      ax[0] = 1.0;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);

      ax[3] = EbsdLib::Constants::k_Pi + 1.0;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }

    {
      using TestType = QVector<float>;
      OrientationTransformation::ResultType result;
      FloatQVectorType ax(4);
      ax[0] = 0.0f;
      ax[1] = 0.0f;
      ax[2] = 1.0f;
      ax[3] = EbsdLib::Constants::k_Pi - 0.00001f;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      ax[0] = 1.0f;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);

      ax[3] = EbsdLib::Constants::k_Pi + 1.0f;
      result = OrientationTransformation::ax_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -1);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_om_check()
  {
    {
      using TestType = FOrientArrayType;
      OrientationTransformation::ResultType result;
      FOrientArrayType ax(9);
      ax[0] = 1.0f;
      ax[1] = 0.0f;
      ax[2] = 0.0f;
      ax[3] = 0.0f;
      ax[4] = 1.0f;
      ax[5] = 0.0f;
      ax[6] = 0.0f;
      ax[7] = 0.0f;
      ax[8] = 1.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      ax[1] = -16.0f;
      ax[6] = -12.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -3);

      ax[3] = EbsdLib::Constants::k_Pi + 1.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);
    }

    {
      using TestType = std::vector<float>;
      OrientationTransformation::ResultType result;
      FloatVectorType ax(9);
      ax[0] = 1.0f;
      ax[1] = 0.0f;
      ax[2] = 0.0f;
      ax[3] = 0.0f;
      ax[4] = 1.0f;
      ax[5] = 0.0f;
      ax[6] = 0.0f;
      ax[7] = 0.0f;
      ax[8] = 1.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      ax[1] = -16.0f;
      ax[6] = -12.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -3);

      ax[3] = EbsdLib::Constants::k_Pi + 1.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);
    }

    {
      using TestType = QVector<float>;
      OrientationTransformation::ResultType result;
      FloatQVectorType ax(9);
      ax[0] = 1.0f;
      ax[1] = 0.0f;
      ax[2] = 0.0f;
      ax[3] = 0.0f;
      ax[4] = 1.0f;
      ax[5] = 0.0f;
      ax[6] = 0.0f;
      ax[7] = 0.0f;
      ax[8] = 1.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, 1);

      ax[1] = -16.0f;
      ax[6] = -12.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -3);

      ax[3] = EbsdLib::Constants::k_Pi + 1.0f;
      result = OrientationTransformation::om_check<TestType>(ax);
      DREAM3D_REQUIRE_EQUAL(result.result, -2);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T, typename K>
  void GenRotTest(K* in, K omega)
  {
    T eu(3);
    eu[0] = in[0];
    eu[1] = in[1];
    eu[2] = in[2];

    T res(4);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_GenRot()
  {

    float eu[3] = {1.0f, 0.0f, 0.0f};
    float omega = EbsdLib::Constants::k_PiOver2;
    GenRotTest<FOrientArrayType, float>(eu, omega);
    GenRotTest<FloatVectorType, float>(eu, omega);
    GenRotTest<FloatQVectorType, float>(eu, omega);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T, typename FloatType>
  void EU_2_XXX(FloatType* in)
  {

    using TestType = T;

    T eu(3);
    eu[0] = in[0];
    eu[1] = in[1];
    eu[2] = in[2];

    eu[0] = fmod(eu[0], EbsdLib::Constants::k_2Pi);
    eu[1] = fmod(eu[1], EbsdLib::Constants::k_Pi);
    eu[2] = fmod(eu[2], EbsdLib::Constants::k_2Pi);

    T res(9);

    typename OrientationTransformation::ResultType result;
    result = OrientationTransformation::eu_check<TestType>(eu);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }

    T ax = OrientationTransformation::eu2ax<T, T>(eu);
    OrientationPrinters::Print_AX<T>(ax);
    result = OrientationTransformation::ax_check<TestType>(ax);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }

    T ro(4);
    ro = OrientationTransformation::eu2ro<T, T>(eu);
    OrientationPrinters::Print_RO<T, FloatType>(ro);
    result = OrientationTransformation::ro_check<TestType>(ro);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }

    T ho(3);
    ho = OrientationTransformation::eu2ho<T, T>(eu);
    OrientationPrinters::Print_HO<T, FloatType>(ho);
    result = OrientationTransformation::ho_check<TestType>(ho);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }

    T cu(3);
    cu = OrientationTransformation::eu2cu<T, T>(eu);
    OrientationPrinters::Print_CU<T>(cu);
    result = OrientationTransformation::cu_check<TestType>(cu);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }

    std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
    cu = OrientationTransformation::ho2cu<T, T>(ho);
    OrientationPrinters::Print_CU<T>(cu);
    result = OrientationTransformation::cu_check<TestType>(cu);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }

    QuatF qu = OrientationTransformation::eu2qu<T, QuatF>(eu);
    OrientationPrinters::Print_QU<QuatF>(qu);
    result = OrientationTransformation::qu_check<QuatF>(qu);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }

    T om(9);
    om = OrientationTransformation::eu2om<T, T>(eu);
    OrientationPrinters::Print_OM<T>(om);
    result = OrientationTransformation::om_check<TestType>(om);
    if(result.result <= 0)
    {
      std::cout << result.msg << std::endl;
    }
  }

  /*
Starting Test ax2eu  eu2ax  -----------------------------------------------------
Total Tuples: 4913
Delta Failed: 1.99037 DataArray: 'ax Difference' Tuple[263] Comp[1] Value:-1.99037
eu
0.0000000000000000   2.9452431201934814   3.1415927410125732
ax
-0.0000000435009078  -0.9951847195625305   0.0980171412229538   3.1415927410125732
0_ax2eu
6.2831850051879883   2.9452428817749023   3.1415922641754150
1_eu2ax
-0.0000000435009078   0.9951847195625305  -0.0980172604322433   3.1415925025939941
------------------------------------------
ax2eu  eu2ax                                                     FAILED
*/

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename K>
  void Test_eu2_XXX()
  {
    using OrientType = Orientation<K>;
    using VectorType = std::vector<K>;
    using QVectorType = QVector<K>;
    {
      K eu[3] = {static_cast<K>(0.3926990816987242L), static_cast<K>(0.0L), static_cast<K>(0.0L)};
      OrientationPrinters::Print_EU<K*>(eu);
      EU_2_XXX<OrientType, K>(eu);
      EU_2_XXX<VectorType, K>(eu);
      EU_2_XXX<QVectorType, K>(eu);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T, typename K>
  void OM_2_XXX(K* in)
  {
    std::cout << "   " << std::endl;
    T om(9);
    for(size_t i = 0; i < 9; i++)
    {
      om[i] = in[i];
    }
    T res(3); // Just size to 9 as we are going to reuse the variable
    // Convert to Euler
    res = OrientationTransformation::om2eu<T, T>(om);
    OrientationPrinters::Print_EU<T>(res);

    // Convert to Rodriques
    res = OrientationTransformation::om2ro<T, T>(om);
    OrientationPrinters::Print_RO<T, K>(res);

    // Convert to Quaternion
    Quaternion<K> quat = OrientationTransformation::om2qu<T, Quaternion<K>>(om);
    OrientationPrinters::Print_QU<Quaternion<K>>(quat);

    // Convert to Axis Angle
    res = OrientationTransformation::om2ax<T, T>(om);
    OrientationPrinters::Print_AX<T>(res);

    // Convert to Homochoric
    res = OrientationTransformation::om2ho<T, T>(om);
    OrientationPrinters::Print_HO<T>(res);

    // Convert to HomoChoric
    res = OrientationTransformation::om2cu<T, T>(om);
    OrientationPrinters::Print_CU<T>(res);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_om2_XXX()
  {
    /*
----------------------------------------------------
Euler angles                     :  90.0000000    0.0000000    0.0000000
Axis angle pair [n; angle]       :  -0.0000000   -0.0000000   -1.0000000 ;   90.0000000
Rodrigues vector                 :      -0.0000000       -0.0000000       -1.0000000
Homochoric representation        :  -0.0000000   -0.0000000   -0.7536693
Cubochoric representation        :   0.0000000    0.0000000   -0.6074544
Quaternion                       :   0.7071068   -0.0000000   -0.0000000   -0.7071068
                                 /  0.0000   1.0000   0.0000 \
Orientation Matrix               : | -1.0000   0.0000   0.0000 |
                                 \  0.0000   0.0000   1.0000 /
*/
    std::cout << "Test_om2_XXX  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$" << std::endl;
    float om[9] = {0.0000, 1.0000, 0.0000, -1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000};
    OrientationPrinters::Print_OM<float*>(om);
    OM_2_XXX<FOrientArrayType>(om);
    OM_2_XXX<FloatVectorType>(om);
    OM_2_XXX<FloatQVectorType>(om);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T, typename K>
  void RO_2_XXX(K* in)
  {
    T ro(9);
    for(size_t i = 0; i < 4; i++)
    {
      ro[i] = in[i];
    }

    T res(9); // Just size to 9 as we are going to reuse the variable

    // Convert to Euler
    res = OrientationTransformation::ro2eu<T, T>(ro);
    OrientationPrinters::Print_EU<T>(res);

    // Convert to Orientation Matrix
    res = OrientationTransformation::ro2om<T, T>(ro);
    OrientationPrinters::Print_OM<T>(res);

    // Convert to Axis Angle
    res = OrientationTransformation::ro2ax<T, T>(ro);
    OrientationPrinters::Print_AX<T>(res);

    // Convert to Quaternion

    Quaternion<K> quat = OrientationTransformation::ro2qu<T, Quaternion<K>>(ro);
    OrientationPrinters::Print_QU<Quaternion<K>>(quat);

    // Convert to Homochoric
    res = OrientationTransformation::ro2ho<T, T>(ro);
    OrientationPrinters::Print_HO<T>(res);

    // Convert to HomoChoric
    res = OrientationTransformation::ro2cu<T, T>(ro);
    OrientationPrinters::Print_CU<T>(res);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_ro2_XXX()
  {
    std::cout << "Test_ro2_XXX  @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" << std::endl;
    float ro[4] = {0.0f, 0.0f, -1.0f, 1.0f};
    OrientationPrinters::Print_RO<float*, float>(ro);
    RO_2_XXX<FOrientArrayType>(ro);
    RO_2_XXX<FloatVectorType>(ro);
    RO_2_XXX<FloatQVectorType>(ro);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T, typename K>
  void AX_2_XXX(K* in)
  {
    T ax(4);
    ax[0] = in[0];
    ax[1] = in[1];
    ax[2] = in[2];
    ax[3] = in[3];

    T res(9);
    // Convert to Orientation Matrix
    res = OrientationTransformation::ax2om<T, T>(ax);
    OrientationPrinters::Print_OM<T>(res);

    // Convert to Axis Angle
    res = OrientationTransformation::ax2eu<T, T>(ax);
    OrientationPrinters::Print_EU<T>(res);

    // Convert to Rodriques
    res = OrientationTransformation::ax2ro<T, T>(ax);
    OrientationPrinters::Print_RO<T, K>(res);

    // Convert to Quaternion
    Quaternion<K> quat = OrientationTransformation::ax2qu<T, Quaternion<K>>(ax);
    OrientationPrinters::Print_QU<Quaternion<K>>(quat);

    // Convert to homochoric
    res = OrientationTransformation::ax2ho<T, T>(ax);
    OrientationPrinters::Print_HO<T>(res);

    // Convert to HomoChoric
    res = OrientationTransformation::ax2cu<T, T>(ax);
    OrientationPrinters::Print_CU<T>(res);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_ax2_XXX()
  {
    std::cout << "Test_ax2_XXX  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << std::endl;
    float ax[4] = {0.0f, 0.0f, -1.0f, static_cast<float>(k_PiOver2)};
    OrientationPrinters::Print_AX<float*>(ax);
    AX_2_XXX<FOrientArrayType>(ax);
    AX_2_XXX<std::vector<float>>(ax);
    AX_2_XXX<FloatQVectorType>(ax);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T, typename K>
  void QU_2_XXX(K* in, typename Quaternion<K>::Order layout = Quaternion<K>::Order::VectorScalar)
  {
    using QuatType = Quaternion<K>;
    QuatType qu(in[0], in[1], in[2], in[3]);

    // Convert to Orientation Matrix
    T res = OrientationTransformation::qu2om<QuatType, T>(qu, layout);
    OrientationPrinters::Print_OM<T>(res);

    // Convert to Axis Angle
    res = OrientationTransformation::qu2eu<QuatType, T>(qu, layout);
    OrientationPrinters::Print_EU<T>(res);

    // Convert to Rodriques
    res = OrientationTransformation::qu2ro<QuatType, T>(qu, layout);
    OrientationPrinters::Print_RO<T, K>(res);

    // Convert to Quaternion
    res = OrientationTransformation::qu2ax<QuatType, T>(qu, layout);
    OrientationPrinters::Print_AX<T>(res);

    // Convert to Homochoric
    res = OrientationTransformation::qu2ho<QuatType, T>(qu, layout);
    OrientationPrinters::Print_HO<T>(res);

    // Convert to HomoChoric
    res = OrientationTransformation::qu2cu<QuatType, T>(qu, layout);
    OrientationPrinters::Print_CU<T>(res);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_qu2_XXX()
  {
    {
      std::cout << "Test_qu2_XXX  (SCALAR, <X, Y, Z>) ***************************************" << std::endl;
      float qu[4] = {static_cast<float>(EbsdLib::Constants::k_1OverRoot2), 0.0f, 0.0f, static_cast<float>(-EbsdLib::Constants::k_1OverRoot2)};
      OrientationPrinters::Print_QU<float*, float>(qu, QuatF::Order::ScalarVector);
      QU_2_XXX<FOrientArrayType>(qu, QuatF::Order::ScalarVector);
      //  QU_2_XXX<std::vector<float> >(qu);
      //  QU_2_XXX<FloatQVectorType>(qu);
    }

    {
      std::cout << "Test_qu2_XXX  (<X, Y, Z>, SCALAR) ***************************************" << std::endl;
      float qu[4] = {0.0f, 0.0f, static_cast<float>(-EbsdLib::Constants::k_1OverRoot2), static_cast<float>(EbsdLib::Constants::k_1OverRoot2)};
      OrientationPrinters::Print_QU<float*, float>(qu);
      QU_2_XXX<FOrientArrayType>(qu);
      //  QU_2_XXX<std::vector<float> >(qu);
      //  QU_2_XXX<FloatQVectorType>(qu);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  template <typename T, typename K>
  void HO_2_XXX(K* in)
  {
    using QuatType = Quaternion<K>;

    T ho(3);
    ho[0] = in[0];
    ho[1] = in[1];
    ho[2] = in[2];

    T res(9);

    // Convert to Euler
    res = OrientationTransformation::ho2eu<T, T>(ho);
    OrientationPrinters::Print_EU<T>(res);

    // Convert to Orientation Matrix
    res = OrientationTransformation::ho2om<T, T>(ho);
    OrientationPrinters::Print_OM<T>(res);

    // Convert to Axis Angle
    res = OrientationTransformation::ho2ax<T, T>(ho);
    OrientationPrinters::Print_AX<T>(res);

    // Convert to Rodriques
    res = OrientationTransformation::ho2ro<T, T>(ho);
    OrientationPrinters::Print_RO<T, K>(res);

    // Convert to Quaternion
    QuatType quat = OrientationTransformation::ho2qu<T, QuatType>(ho);
    OrientationPrinters::Print_QU<QuatType>(quat);

    // Convert to HomoChoric
    res = OrientationTransformation::ho2cu<T, T>(ho);
    OrientationPrinters::Print_CU<T>(res);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void Test_ho2_XXX()
  {
    std::cout << "Test_ho2_XXX  &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&" << std::endl;
    float ho[3] = {0.000000f, 0.000000f, -0.7536693215f};
    OrientationPrinters::Print_HO<float*>(ho);
    HO_2_XXX<FOrientArrayType>(ho);
    HO_2_XXX<std::vector<float>>(ho);
    HO_2_XXX<FloatQVectorType>(ho);
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void TestInputs()
  {
    std::vector<size_t> cDims(1, 3);
    EbsdLib::FloatArrayType::Pointer data = EbsdLib::FloatArrayType::CreateArray(2, cDims, "Eulers", true);
    data->initializeWithZeros();
    float* fPtr = data->getPointer(0);
    fPtr[0] = 90.0 * EbsdLib::Constants::k_PiOver180;
    fPtr[1] = 0.0;
    fPtr[2] = 0.0;
    fPtr[3] = 90.0F * EbsdLib::Constants::k_PiOver180;
    fPtr[4] = 0.0;
    fPtr[5] = 0.0;

    //& Notation
    {
      FOrientArrayType eu(&(fPtr[0]), 3); // Wrap the pointer with the &notation
      eu[0] = 45.0f * EbsdLib::Constants::k_PiOver180;
      eu[1] = 90.0f * EbsdLib::Constants::k_PiOver180;
      eu[2] = 135.0f * EbsdLib::Constants::k_PiOver180;

      DREAM3D_REQUIRE_EQUAL(eu[0], fPtr[0]);
      DREAM3D_REQUIRE_EQUAL(eu[1], fPtr[1]);
      DREAM3D_REQUIRE_EQUAL(eu[2], fPtr[2]);
    }

    // Pointer Arithmetic (inputs)
    {
      FOrientArrayType eu(fPtr + 3, 3);
      eu[0] = 135.0f * EbsdLib::Constants::k_PiOver180;
      eu[1] = 45.0f * EbsdLib::Constants::k_PiOver180;
      eu[2] = 90.0f * EbsdLib::Constants::k_PiOver180;

      DREAM3D_REQUIRE_EQUAL(eu[0], fPtr[3]);
      DREAM3D_REQUIRE_EQUAL(eu[1], fPtr[4]);
      DREAM3D_REQUIRE_EQUAL(eu[2], fPtr[5]);
    }

    // Pointer Arithmetic, placing results directly into an array
    {
      FOrientArrayType ax(0.0f, 0.0f, -1.0f, EbsdLib::Constants::k_PiOver2);
      FOrientArrayType eu = OrientationTransformation::ax2eu<FOrientArrayType, FOrientArrayType>(ax);
      eu.copyInto(fPtr + 3, 3);
      DREAM3D_REQUIRE_EQUAL(eu[0], fPtr[3]);
      DREAM3D_REQUIRE_EQUAL(eu[1], fPtr[4]);
      DREAM3D_REQUIRE_EQUAL(eu[2], fPtr[5]);

      bool b = EbsdLibMath::closeEnough(eu[0], fPtr[3]);
      DREAM3D_REQUIRE_EQUAL(b, true)
      b = EbsdLibMath::closeEnough(eu[1], fPtr[4]);
      DREAM3D_REQUIRE_EQUAL(b, true)
      b = EbsdLibMath::closeEnough(eu[2], fPtr[5]);
      DREAM3D_REQUIRE_EQUAL(b, true)
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void QuatTest()
  {
    QuatF q(-0.5f, -0.5f, -0.5f, 0.5f);
    using VecType = std::vector<float>;
    VecType qVec = {-0.5f, -0.5f, -0.5f, 0.5f};

    VecType gVec = OrientationTransformation::qu2om<VecType, VecType>(qVec);

    OrientationPrinters::Print_OM<std::vector<float>>(gVec);

    float g[3][3];

    g[0][0] = gVec[0];
    g[0][1] = gVec[1];
    g[0][2] = gVec[2];
    g[1][0] = gVec[3];
    g[1][1] = gVec[4];
    g[1][2] = gVec[5];
    g[2][0] = gVec[6];
    g[2][1] = gVec[7];
    g[2][2] = gVec[8];

    for(size_t r = 0; r < 3; r++)
    {
      for(size_t c = 0; c < 3; c++)
      {
        g[c][r] = gVec[3 * r + c];
        std::cout << c << "," << r << "=" << g[c][r] << std::endl;
      }
    }

    std::array<float, 3> vg;
    std::array<float, 3> v = {1.0f, 0.0f, 0.0f};

    // Mathematically correct, results unintuitive
    std::array<float, 3> vq = q.multiplyByVector(v.data());

    std::cout << "vq: " << vq[0] << "," << vq[1] << "," << vq[2] << std::endl;

    EbsdMatrixMath::Multiply3x3with3x1(g, v.data(), vg.data());
    std::cout << "vg: " << vg[0] << "," << vg[1] << "," << vg[2] << std::endl;
  }

  // -----------------------------------------------------------------------------
  void operator()()
  {
    std::cout << "<===== Start " << getNameOfClass() << std::endl;

    int err = EXIT_SUCCESS;
    DREAM3D_REGISTER_TEST(TestRotArray());
    DREAM3D_REGISTER_TEST(Test_eu_check());
    DREAM3D_REGISTER_TEST(Test_ro_check());
    DREAM3D_REGISTER_TEST(Test_ho_check());
    DREAM3D_REGISTER_TEST(Test_cu_check());
    DREAM3D_REGISTER_TEST(Test_qu_check());
    DREAM3D_REGISTER_TEST(Test_ax_check());
    DREAM3D_REGISTER_TEST(Test_om_check());

    DREAM3D_REGISTER_TEST(Test_GenRot())

    DREAM3D_REGISTER_TEST(Test_eu2_XXX<double>());
    DREAM3D_REGISTER_TEST(Test_eu2_XXX<float>());

    DREAM3D_REGISTER_TEST(Test_ax2_XXX());
    DREAM3D_REGISTER_TEST(Test_om2_XXX());
    DREAM3D_REGISTER_TEST(Test_ro2_XXX());
    DREAM3D_REGISTER_TEST(Test_qu2_XXX());
    DREAM3D_REGISTER_TEST(Test_ho2_XXX());

    DREAM3D_REGISTER_TEST(TestInputs());
  }
};


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/math/quaternion.hpp>
#include <cmath>
#include <limits>
#include <pup.h>
#include <string>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "Domain/FunctionsOfTime/FunctionOfTime.hpp"
#include "Domain/FunctionsOfTime/FunctionOfTimeHelpers.hpp"
#include "Domain/FunctionsOfTime/PiecewisePolynomial.hpp"
#include "Parallel/CharmPupable.hpp"
#include "Utilities/Gsl.hpp"

namespace domain::FunctionsOfTime {
/// \ingroup ComputationalDomainGroup
/// \brief A FunctionOfTime that stores quaternions for the rotation map
///
/// \details This FunctionOfTime stores quaternions that will be used in the
/// time-dependent rotation map as well as the orbital angular velocity that
/// will be controlled by the rotation control sytem. To get the quaternion, an
/// ODE is solved of the form \f$ \dot{q} = \frac{1}{2} q \times \omega \f$
/// where \f$ \omega \f$ is the orbital angular velocity which is stored
/// internally as the derivative of an angle `PiecewisePolynomial`, and
/// \f$ \times \f$ here is quaternion multiplication.
///
/// Different from a `PiecewisePolynomial`, only the quaternion
/// itself is stored, not any of the derivatives because the derivatives must be
/// calculated from the solved ODE at every function call. Because
/// derivatives of the quaternion are not stored, the template parameter
/// `MaxDeriv` refers to both the max derivative of the stored angle
/// PiecewisePolynomial and the max derivative returned by the
/// QuaternionFunctionOfTime. The `update` function is then just a wrapper
/// around the internal `PiecewisePolynomial::update` function with the addition
/// that it then updates the stored quaternions as well.
///
/// The angle PiecewisePolynomial is accessible through the `angle_func`,
/// `angle_func_and_deriv`, and `angle_func_and_2_derivs` functions which
/// correspond to the function calls of a normal PiecewisePolynomial except
/// without the `angle_` prefix.
///
/// It is encouraged to use `quat_func` and `angle_func` when you want the
/// specific values of the functions to avoid ambiguity in what you are
/// calling. However, the original three `func` functions inherited from the
/// FunctionOfTime base class are necessary because the maps use the generic
/// `func` functions, thus they return the quaternion and its derivatives (which
/// are needed for the map). This is all to keep the symmetry of naming
/// `angle_func` and `quat_func` so that function calls won't be ambiguous.
template <size_t MaxDeriv>
class QuaternionFunctionOfTime : public FunctionOfTime {
 public:
  QuaternionFunctionOfTime() = default;
  QuaternionFunctionOfTime(
      double t, std::array<DataVector, 1> initial_quat_func,
      std::array<DataVector, MaxDeriv + 1> initial_angle_func,
      double expiration_time);

  ~QuaternionFunctionOfTime() override = default;
  QuaternionFunctionOfTime(QuaternionFunctionOfTime&&) = default;
  QuaternionFunctionOfTime& operator=(QuaternionFunctionOfTime&&) = default;
  QuaternionFunctionOfTime(const QuaternionFunctionOfTime&) = default;
  QuaternionFunctionOfTime& operator=(const QuaternionFunctionOfTime&) =
      default;

  // LCOV_EXCL_START
  explicit QuaternionFunctionOfTime(CkMigrateMessage* /*unused*/) {}
  // LCOV_EXCL_STOP

  auto get_clone() const -> std::unique_ptr<FunctionOfTime> override;

  // clang-tidy: google-runtime-references
  // clang-tidy: cppcoreguidelines-owning-memory,-warnings-as-errors
  WRAPPED_PUPable_decl_template(QuaternionFunctionOfTime<MaxDeriv>);  // NOLINT

  void reset_expiration_time(const double next_expiration_time) override {
    angle_f_of_t_.reset_expiration_time(next_expiration_time);
  }

  /// Returns domain of validity for the function of time
  std::array<double, 2> time_bounds() const override {
    return angle_f_of_t_.time_bounds();
  }

  /// Updates the `MaxDeriv`th derivative of the angle piecewisepolynomial at
  /// the given time, then updates the stored quaternions.
  ///
  /// `updated_max_deriv` is a datavector of the `MaxDeriv`s for each component.
  /// `next_expiration_time` is the next expiration time.
  void update(double time_of_update, DataVector updated_max_deriv,
              double next_expiration_time) override;

  // NOLINTNEXTLINE(google-runtime-references)
  void pup(PUP::er& p) override;

  /// Returns the quaternion at an arbitrary time `t`.
  std::array<DataVector, 1> func(const double t) const override {
    return quat_func(t);
  }

  /// Returns the quaternion and its first derivative at an arbitrary time `t`.
  std::array<DataVector, 2> func_and_deriv(const double t) const override {
    return quat_func_and_deriv(t);
  }

  /// Returns the quaternion and the first two derivatives at an arbitrary
  /// time `t`.
  std::array<DataVector, 3> func_and_2_derivs(const double t) const override {
    return quat_func_and_2_derivs(t);
  }

  /// Returns the quaternion at an arbitrary time `t`.
  std::array<DataVector, 1> quat_func(double t) const;

  /// Returns the quaternion and its first derivative at an arbitrary time `t`.
  std::array<DataVector, 2> quat_func_and_deriv(double t) const;

  /// Returns the quaternion and the first two derivatives at an arbitrary
  /// time `t`.
  std::array<DataVector, 3> quat_func_and_2_derivs(double t) const;

  /// Returns stored angle at an arbitrary time `t`.
  std::array<DataVector, 1> angle_func(const double t) const {
    return angle_f_of_t_.func(t);
  }

  /// Returns stored angle and its first derivative (omega) at an arbitrary time
  /// `t`.
  std::array<DataVector, 2> angle_func_and_deriv(const double t) const {
    return angle_f_of_t_.func_and_deriv(t);
  }

  /// Returns stored angle and the first two derivatives at an arbitrary
  /// time `t`.
  std::array<DataVector, 3> angle_func_and_2_derivs(const double t) const {
    return angle_f_of_t_.func_and_2_derivs(t);
  }

 private:
  template <size_t LocalMaxDeriv>
  friend bool operator==(  // NOLINT(readability-redundant-declaration)
      const QuaternionFunctionOfTime<LocalMaxDeriv>& lhs,
      const QuaternionFunctionOfTime<LocalMaxDeriv>& rhs);

  template <size_t LocalMaxDeriv>
  friend std::ostream& operator<<(  // NOLINT(readability-redundant-declaration)
      std::ostream& os,
      const QuaternionFunctionOfTime<LocalMaxDeriv>& quaternion_f_of_t);

  std::vector<FunctionOfTimeHelpers::StoredInfo<1, false>>
      stored_quaternions_and_times_;

  domain::FunctionsOfTime::PiecewisePolynomial<MaxDeriv> angle_f_of_t_;

  /// Integrates the ODE \f$ \dot{q} = \frac{1}{2} q \times \omega \f$ from time
  /// `t0` to time `t`. On input, `quaternion_to_integrate` is the initial
  /// quaternion at time `t0` and on output, it stores the result at time `t`
  void solve_quaternion_ode(
      gsl::not_null<boost::math::quaternion<double>*> quaternion_to_integrate,
      double t0, double t) const;

  /// Updates the `std::vector<StoredInfo>` to have the same number of stored
  /// quaternions as the `angle_f_of_t_ptr` has stored angles. This is necessary
  /// to ensure we can solve the ODE at any time `t`
  void update_stored_info();

  /// Does common operations to all the `func` functions such as updating stored
  /// info, solving the ODE, and returning the normalized quaternion as a boost
  /// quaternion for easy calculations
  boost::math::quaternion<double> setup_func(double t) const;
};

template <size_t MaxDeriv>
bool operator!=(const QuaternionFunctionOfTime<MaxDeriv>& lhs,
                const QuaternionFunctionOfTime<MaxDeriv>& rhs);

/// \cond
template <size_t MaxDeriv>
PUP::able::PUP_ID QuaternionFunctionOfTime<MaxDeriv>::my_PUP_ID = 0;  // NOLINT
/// \endcond
}  // namespace domain::FunctionsOfTime


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Col;
using arma::span;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
	class ExpectedGenColVecElemIndRange : public Expected {
    public:
		ExpectedGenColVecElemIndRange() {
			cout << "Compute ExpectedGenColElemIndRange(): " << endl;

			vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
				InputClass::GenColVec,
				InputClass::ElemIndRange
			});

			for (vector<pair<string, void*>> input : inputs) {
				_fileSuffix = "";

				int n = 0;
				for (pair<string, void*> value : input) {
					switch (n) {
						case 0:
							_fileSuffix += value.first;
							_genColVec = *static_cast<Col<double>*>(value.second);
							break;
						case 1:
							_fileSuffix += "," + value.first;
							_elemIndRange = *static_cast<span*>(value.second);
							break;
					}
					++n;
				}

				cout << "Using input: " << _fileSuffix << endl;

				expectedColVecIn_range();
				expectedColVecRows();
				expectedColVecSubvec();
			}

			cout << "done." << endl;
        }

    protected:
		Col<double> _genColVec;
		span _elemIndRange;

		void expectedColVecIn_range() {
			cout << "- Compute expectedColIn_range() ... ";

			if(_genColVec.in_range(_elemIndRange)) {
				save<double>("Col.in_range", Col<double>({1}));
			} else {
				save<double>("Col.in_range", Col<double>({0}));
			}

			cout << "done." << endl;
		}

		void expectedColVecRows() {
            if(_elemIndRange.whole) {
              return;
            }

			cout << "- Compute expectedColRows() ... ";

			if(_genColVec.in_range(_elemIndRange)) {
				save<double>("Col.rows", _genColVec.rows(_elemIndRange.a, _elemIndRange.b));
			}

			cout << "done." << endl;
		}

		void expectedColVecSubvec() {
            if(_elemIndRange.whole) {
              return;
            }

			cout << "- Compute expectedColSubvec() ... ";

			if(_genColVec.in_range(_elemIndRange)) {
				save<double>("Col.subvec", _genColVec.subvec(_elemIndRange.a, _elemIndRange.b));
			}

			cout << "done." << endl;
		}
	};
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller.h>
#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/serialization/single_buffered.h>
#include <boost/python/make_constructor.hpp>
#include <boost/optional.hpp>
#include <tbxx/error_utils.hpp>

namespace scitbx { namespace serialization { namespace single_buffered {

  inline
  char* to_string(char* start, cctbx::miller::index<> const& value)
  {
    return
      to_string(to_string(to_string(start, value[0]), value[1]), value[2]);
  }

  template <>
  struct from_string<cctbx::miller::index<> >
  {
    from_string(const char* start)
    {
      end = start;
      for(std::size_t i=0;i<3;i++) {
        from_string<int> proxy(end);
        value[i] = proxy.value;
        end = proxy.end;
      }
    }

    cctbx::miller::index<> value;
    const char* end;
  };

}}} // namespace scitbx::serialization::single_buffered

#include <scitbx/array_family/boost_python/flex_pickle_single_buffered.h>

namespace scitbx { namespace af { namespace boost_python {

namespace {

  flex<cctbx::miller::index<> >::type*
  join(
    af::const_ref<int> const& h,
    af::const_ref<int> const& k,
    af::const_ref<int> const& l)
  {
    SCITBX_ASSERT(k.size() == h.size());
    SCITBX_ASSERT(l.size() == h.size());
    af::shared<cctbx::miller::index<> > result((af::reserve(h.size())));
    for(std::size_t i=0;i<h.size();i++) {
      result.push_back(cctbx::miller::index<>(h[i],k[i],l[i]));
    }
    return new flex<cctbx::miller::index<> >::type(result, result.size());
  }

  af::shared<vec3<double> >
  as_vec3_double(af::const_ref<cctbx::miller::index<> > const& a)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(vec3<double>(a[i]));
    }
    return result;
  }

  double
  fourier_transform_real_part_at_x(
    af::const_ref<cctbx::miller::index<> > const& miller_indices,
    af::const_ref<std::complex<double> > const& fourier_coeffs,
    af::tiny<double, 3> const& x)
  {
    TBXX_ASSERT(fourier_coeffs.size() == miller_indices.size());
    double result = 0;
    double mtp = -constants::two_pi;
    for(std::size_t i=0;i<miller_indices.size();i++) {
      cctbx::miller::index<> const& h = miller_indices[i];
      double mtphx = mtp * (h[0]*x[0] + h[1]*x[1] + h[2]*x[2]);
      double a = std::cos(mtphx);
      double b = std::sin(mtphx);
      std::complex<double> const& fc = fourier_coeffs[i];
      result += fc.real() * a - fc.imag() * b;
    }
    return result;
  }

  boost::optional<std::size_t>
  first_index(
    af::const_ref<cctbx::miller::index<> > const& miller_indices,
    af::tiny<double, 3> const& x)
  {
    boost::optional<std::size_t> result ;
    for(std::size_t i=0;i<miller_indices.size();i++) {
      cctbx::miller::index<> const& h = miller_indices[i];
      if ((h[0]==x[0]) && (h[1]==x[1]) && (h[2]==x[2])) {
        result = i;
        break;
      };
    };
    return result;
  }

} // namespace <anonymous>

  void wrap_flex_miller_index(boost::python::object const& flex_root_scope)
  {
    namespace bp = boost::python;
    namespace miller = cctbx::miller;

    flex_wrapper<miller::index<> >::ordered("miller_index", flex_root_scope)
      .def("__init__", bp::make_constructor(join))
      .def("__neg__", flex_wrapper<miller::index<> >::neg_a)
      .def_pickle(flex_pickle_single_buffered<miller::index<>,
        3*pickle_size_per_element<miller::index<>::value_type>::value>())
      .def("as_vec3_double", as_vec3_double)
      .def("fourier_transform_real_part_at_x",
        fourier_transform_real_part_at_x, (
          bp::arg("fourier_coeffs"), bp::arg("x")))
      .def("first_index",
        first_index, (
          bp::arg("miller_index")))
    ;
  }

}}} // namespace scitbx::af::boost_python


#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/preprocessor/seq/for_each.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/static_matrix.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>
#include <amgcl/preconditioner/cpr.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/block_matrix.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

//---------------------------------------------------------------------------
template <class Matrix>
void solve_cpr(const Matrix &K, const std::vector<double> &rhs, boost::property_tree::ptree &prm)
{
    auto t1 = prof.scoped_tic("CPR");

    typedef amgcl::backend::builtin<double> Backend;

    typedef
        amgcl::amg<Backend, amgcl::runtime::coarsening::wrapper, amgcl::runtime::relaxation::wrapper>
        PPrecond;

    typedef
        amgcl::relaxation::as_preconditioner<Backend, amgcl::runtime::relaxation::wrapper>
        SPrecond;

    prof.tic("setup");
    amgcl::make_solver<
        amgcl::preconditioner::cpr<PPrecond, SPrecond>,
        amgcl::runtime::solver::wrapper<Backend>
        > solve(K, prm);
    prof.toc("setup");

    std::cout << solve.precond() << std::endl;

    std::vector<double> x(rhs.size(), 0.0);

    size_t iters;
    double error;

    prof.tic("setup");
    std::tie(iters, error) = solve(rhs, x);
    prof.toc("setup");

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl;
}

//---------------------------------------------------------------------------
template <int B, class Matrix>
void solve_block_cpr(const Matrix &K, const std::vector<double> &rhs, boost::property_tree::ptree &prm)
{
    auto t1 = prof.scoped_tic("CPR");

    typedef amgcl::static_matrix<double, B, B> val_type;
    typedef amgcl::static_matrix<double, B, 1> rhs_type;
    typedef amgcl::backend::builtin<val_type>  SBackend;
    typedef amgcl::backend::builtin<double>    PBackend;

    typedef
        amgcl::amg<
            PBackend,
            amgcl::runtime::coarsening::wrapper,
            amgcl::runtime::relaxation::wrapper>
        PPrecond;

    typedef
        amgcl::relaxation::as_preconditioner<
            SBackend,
            amgcl::runtime::relaxation::wrapper
            >
        SPrecond;

    prof.tic("setup");
    amgcl::make_solver<
        amgcl::preconditioner::cpr<PPrecond, SPrecond>,
        amgcl::runtime::solver::wrapper<SBackend>
        > solve(amgcl::adapter::block_matrix<val_type>(K), prm);
    prof.toc("setup");

    std::cout << solve.precond() << std::endl;

    std::vector<rhs_type> x(rhs.size(), amgcl::math::zero<rhs_type>());

    auto rhs_ptr = reinterpret_cast<const rhs_type*>(rhs.data());
    size_t n = amgcl::backend::rows(K) / B;

    size_t iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = solve(amgcl::make_iterator_range(rhs_ptr, rhs_ptr + n), x);
    prof.toc("solve");

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl;
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    using std::string;
    using std::vector;
    using amgcl::prof;
    using amgcl::precondition;

    namespace po = boost::program_options;
    namespace io = amgcl::io;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "matrix,A",
         po::value<string>()->required(),
         "The system matrix in MatrixMarket format"
        )
        (
         "rhs,f",
         po::value<string>(),
         "The right-hand side in MatrixMarket format"
        )
        (
         "runtime-block-size,b",
         po::value<int>(),
         "The block size of the system matrix set at runtime"
        )
        (
         "static-block-size,c",
         po::value<int>()->default_value(1),
         "The block size of the system matrix set at compiletime"
        )
        (
         "params,P",
         po::value<string>(),
         "parameter file in json format"
        )
        (
         "prm,p",
         po::value< vector<string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    po::notify(vm);

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(vm["params"].as<string>(), prm);

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string> >()) {
            amgcl::put(prm, v);
        }
    }

    int cb = vm["static-block-size"].as<int>();

    if (vm.count("runtime-block-size"))
        prm.put("precond.block_size", vm["runtime-block-size"].as<int>());
    else
        prm.put("precond.block_size", cb);

    size_t rows;
    vector<ptrdiff_t> ptr, col;
    vector<double> val, rhs;
    std::vector<char> pm;

    {
        auto t = prof.scoped_tic("reading");

        string Afile  = vm["matrix"].as<string>();
        bool   binary = vm["binary"].as<bool>();

        if (binary) {
            io::read_crs(Afile, rows, ptr, col, val);
        } else {
            size_t cols;
            std::tie(rows, cols) = io::mm_reader(Afile)(ptr, col, val);
            precondition(rows == cols, "Non-square system matrix");
        }

        if (vm.count("rhs")) {
            string bfile = vm["rhs"].as<string>();

            size_t n, m;

            if (binary) {
                io::read_dense(bfile, n, m, rhs);
            } else {
                std::tie(n, m) = io::mm_reader(bfile)(rhs);
            }

            precondition(n == rows && m == 1, "The RHS vector has wrong size");
        } else {
            rhs.resize(rows, 1.0);
        }
    }

#define CALL_BLOCK_SOLVER(z, data, B)                                          \
    case B:                                                                    \
        solve_block_cpr<B>(std::tie(rows, ptr, col, val), rhs, prm);           \
        break;

    switch(cb) {
        case 1:
            solve_cpr(std::tie(rows, ptr, col, val), rhs, prm);
            break;

        BOOST_PP_SEQ_FOR_EACH(CALL_BLOCK_SOLVER, ~, AMGCL_BLOCK_SIZES)

        default:
            precondition(false, "Unsupported block size");
            break;
    }

    std::cout << prof << std::endl;
}


#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <Eigen/Dense>

class JointToActuator {
 public:
  JointToActuator();

 private:
  void callback(const sensor_msgs::JointState::ConstPtr& joint_state);

  ros::NodeHandle nh_;
  ros::Publisher actuator_pub_;
  ros::Subscriber joint_sub_;
  std::vector<std::string> joint_names_ = {"tool_rotate_joint", "proximal_wrist_joint", "distal_wrist_joint", "jaw_a_joint"};
  std::vector<std::string> actuator_names_ = {"m1", "m2", "m3", "m4"};
  Eigen::MatrixXd actuator_transform_ = {4,4};
};

JointToActuator::JointToActuator() {
  nh_.getParam("joint_names", joint_names_);
  nh_.getParam("actuator_names", actuator_names_);
  actuator_transform_.setIdentity();
  std::vector<double> actuator_transform;
  if (nh_.getParam("joint_actuator_position_transform", actuator_transform)) {
      actuator_transform_ = Eigen::Map<Eigen::MatrixXd>(actuator_transform.data(), joint_names_.size(), actuator_names_.size()).transpose();
  }
  actuator_pub_ = nh_.advertise<sensor_msgs::JointState>("actuator_states", 1, true);
  joint_sub_ = nh_.subscribe<sensor_msgs::JointState>("joint_states", 10, &JointToActuator::callback, this);
}

void JointToActuator::callback(const sensor_msgs::JointState::ConstPtr& joint_state) {
  sensor_msgs::JointState actuator_state;
  Eigen::VectorXd joint_position(joint_names_.size());
  for (int i=0; i<joint_names_.size(); i++) {
      auto joint_name = joint_names_[i];
      auto it = std::find(joint_state->name.begin(), joint_state->name.end(), joint_name);
      if (it == joint_state->name.end()) {
          return;
      }
//      std::cout << it - joint_state->name.begin() << std::endl;
      joint_position(i) = joint_state->position[it - joint_state->name.begin()];
  }

  Eigen::VectorXd actuator_position = actuator_transform_ * joint_position;
  std::vector<double> actuator_position_vector(actuator_position.data(), actuator_position.data() + actuator_position.size());
  actuator_state.position = actuator_position_vector;
  actuator_state.name = actuator_names_;
  actuator_state.header.stamp = joint_state->header.stamp;
  actuator_pub_.publish(actuator_state);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "joint_to_actuator");
  JointToActuator joint_to_actuator;

  ros::spin();
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ATAN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ATAN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse tangent.

    @par Header <boost/simd/function/atan.hpp>

    @par Note

      For every parameter of floating type `atan(x)`
      returns the arc @c r in the interval  \f$[-\pi/2, \pi/2[\f$
      such that <tt>tan(r) == x</tt>.

    @par Decorators

     - std_ for floating entries  provides access to std::atan

    @see atan2, atand, atanpi, tan

    @par Example:

      @snippet atan.cpp atan

    @par Possible output:

      @snippet atan.txt atan

  **/
  IEEEValue atan(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/atan.hpp>
#include <boost/simd/function/simd/atan.hpp>

#endif


/* box.hpp

 * Copyright (C) 2021 SS47816

 * Implementation of the Box and BoxQ class

**/

#pragma once

#include <Eigen/Geometry>

struct Box
{
 public:
	int id;
	Eigen::Vector3f position;
	Eigen::Vector3f dimension;
	Eigen::Quaternionf quaternion;

	Box() {};

	Box(int id, Eigen::Vector3f position, Eigen::Vector3f dimension)
		: id(id), position(position), dimension(dimension)
	{
		quaternion = Eigen::Quaternionf(1, 0, 0, 0);
	}

	Box(int id, Eigen::Vector3f position, Eigen::Vector3f dimension, Eigen::Quaternionf quaternion)
		: id(id), position(position), dimension(dimension), quaternion(quaternion)
	{}
};


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include <boost/math/special_functions/digamma.hpp>

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    template <typename X>
    DigammaFunction<X>::DigammaFunction() {
    }

    template <typename X>
    DigammaFunction<X>::~DigammaFunction() {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    template <typename X>
    double DigammaFunction<X>::getValue(const VariableType& argument) const {
      return boost::math::digamma<X>(argument);
    }

  }
}


#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "../trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {

  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  ifstream fin(trajectory_file);
  if (!fin) {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }
  cout << "read total " << poses.size() << " pose entries" << endl;

  // draw trajectory in pangolin
  DrawTrajectory(poses);
  return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. Distributed under 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_MP_CPP_COMPLEX_HPP
#define BOOST_MP_CPP_COMPLEX_HPP

#include <boost/multiprecision/complex_adaptor.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

namespace boost {
namespace multiprecision {

#ifndef BOOST_NO_CXX11_TEMPLATE_ALIASES

template <unsigned Digits,
          backends::digit_base_type DigitBase = backends::digit_base_10,
          class Allocator = void, class Exponent = int,
          Exponent MinExponent = 0, Exponent MaxExponent = 0>
using cpp_complex_backend =
    complex_adaptor<cpp_bin_float<Digits, DigitBase, Allocator, Exponent,
                                  MinExponent, MaxExponent>>;

template <unsigned Digits, backends::digit_base_type DigitBase = digit_base_10,
          class Allocator = void, class Exponent = int,
          Exponent MinExponent = 0, Exponent MaxExponent = 0,
          expression_template_option ExpressionTemplates = et_off>
using cpp_complex =
    number<complex_adaptor<cpp_bin_float<Digits, DigitBase, Allocator, Exponent,
                                         MinExponent, MaxExponent>>,
           ExpressionTemplates>;

typedef cpp_complex<50> cpp_complex_50;
typedef cpp_complex<100> cpp_complex_100;

typedef cpp_complex<24, backends::digit_base_2, void, boost::int16_t, -126, 127>
    cpp_complex_single;
typedef cpp_complex<53, backends::digit_base_2, void, boost::int16_t, -1022,
                    1023>
    cpp_complex_double;
typedef cpp_complex<64, backends::digit_base_2, void, boost::int16_t, -16382,
                    16383>
    cpp_complex_extended;
typedef cpp_complex<113, backends::digit_base_2, void, boost::int16_t, -16382,
                    16383>
    cpp_complex_quad;
typedef cpp_complex<237, backends::digit_base_2, void, boost::int32_t, -262142,
                    262143>
    cpp_complex_oct;

#else

typedef number<complex_adaptor<cpp_bin_float<50>>, et_off> cpp_complex_50;
typedef number<complex_adaptor<cpp_bin_float<100>>, et_off> cpp_complex_100;

typedef number<complex_adaptor<cpp_bin_float<24, backends::digit_base_2, void,
                                             boost::int16_t, -126, 127>>,
               et_off>
    cpp_complex_single;
typedef number<complex_adaptor<cpp_bin_float<53, backends::digit_base_2, void,
                                             boost::int16_t, -1022, 1023>>,
               et_off>
    cpp_complex_double;
typedef number<complex_adaptor<cpp_bin_float<64, backends::digit_base_2, void,
                                             boost::int16_t, -16382, 16383>>,
               et_off>
    cpp_complex_extended;
typedef number<complex_adaptor<cpp_bin_float<113, backends::digit_base_2, void,
                                             boost::int16_t, -16382, 16383>>,
               et_off>
    cpp_complex_quad;
typedef number<complex_adaptor<cpp_bin_float<237, backends::digit_base_2, void,
                                             boost::int32_t, -262142, 262143>>,
               et_off>
    cpp_complex_oct;

#endif

} // namespace multiprecision
} // namespace boost

#endif


/*
   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 2 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, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

   Product name: redemption, a FLOSS RDP proxy
   Copyright (C) Wallix 2016
   Author(s): Christophe Grosjean

*/

#include "utils/crypto/ssl_mod_exp_direct.hpp"

#include <cassert>
#include <cstddef>

#include "cxx/diagnostic.hpp"

#include <boost/multiprecision/cpp_int.hpp>

/**
 * \pre  \a out_len >= \a modulus_size
 * \return  the length of the big-endian number placed at out. ~size_t{} if error
 */
writable_bytes_view mod_exp_direct(
    writable_bytes_view out,
    bytes_view inr,
    bytes_view modulus,
    bytes_view exponent
) {
    assert(out.size() >= modulus.size());

    using int_type = boost::multiprecision::cpp_int;

    auto b256_to_bigint = [](bytes_view s) {
        int_type i;
        boost::multiprecision::import_bits(i, s.begin(), s.end());
        return i;
    };

    int_type base = b256_to_bigint(inr);
    int_type exp = b256_to_bigint(exponent);
    int_type m = b256_to_bigint(modulus);

    REDEMPTION_DIAGNOSTIC_PUSH()
    REDEMPTION_DIAGNOSTIC_GCC_IGNORE("-Wzero-as-null-pointer-constant")
    int_type r = boost::multiprecision::powm(base, exp, m);
    REDEMPTION_DIAGNOSTIC_POP()

    auto it = boost::multiprecision::export_bits(r, out.data(), 8);
    auto r_len = static_cast<size_t>(it - out.data());
    *it = 0;
    if (r_len == 1 && out[0] == 0) {
        r_len = 0;
    }
    return out.first(r_len);
}


//#define CGAL_USE_BOOST_BIMAP

#include <fstream>
#include <vector>

// CGAL headers

#include "svd-typedefs.h"

#include <CGAL/Timer.h>
#include <boost/config.hpp>
#include <boost/version.hpp>
// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>
#include <QDragEnterEvent>
#include <QDropEvent>
#include <QMessageBox>
// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/SegmentDelaunayGraphLinfGraphicsItem.h>
#include <CGAL/Constraints_loader.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif
//#include <CGAL/Qt/Converter.h>

// the two base classes
#include "ui_Segment_voronoi_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

// for viewportsBbox(QGraphicsScene*)
#include <CGAL/Qt/utility.h>

typedef Rep K;
typedef SDG_2 SVD;

typedef SVD::Vertex_handle Vertex_handle;

// global variables
bool has_file_argument;
QString file_to_open;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Segment_voronoi_2
{
  Q_OBJECT

private:
  SVD svd;
  QGraphicsScene scene;
  std::list<Point_2> seeds;

  CGAL::Qt::SegmentDelaunayGraphLinfGraphicsItem<SVD> * sdggi;

  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

public:
  MainWindow();

private:
  template <typename Iterator>
  void insert_polyline(Iterator b, Iterator e)
  {
    Point_2 p, q;
    SVD::Vertex_handle vh, wh;
    Iterator it = b;
    vh = svd.insert(*it);
    p = *it;
    ++it;
    for(; it != e; ++it){
      q = *it;
      if(p != q){
        wh = svd.insert(*it);
        svd.insert(vh,wh);
        vh = wh;
        p = q;
      } else {
        std::cout << "duplicate point: " << p << std::endl;
      }
    }
    Q_EMIT( changed());
  }

protected Q_SLOTS:
 virtual void open(QString);

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void on_actionInsertPolyline_toggled(bool checked);

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  void on_actionLoadSegments_triggered();

  void loadPolygonConstraints(QString);

  void loadEdgConstraints(QString);

  void loadPoints(QString);

  void loadWKT(QString);

  void loadPointsInput(QString);

  void loadSitesInput(QString);

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  // Add a GraphicItem for the SVD triangulation
  sdggi = new CGAL::Qt::SegmentDelaunayGraphLinfGraphicsItem<SVD>(&svd);
  QColor segmentColor(::Qt::blue);
  QColor voronoiColor(::Qt::black);
  segmentColor.setAlpha(150);
  sdggi->setSegmentPen(QPen(segmentColor,0));
  sdggi->setVoronoiPen(QPen(voronoiColor,0));

  QObject::connect(this, SIGNAL(changed()),
		   sdggi, SLOT(modelChanged()));

  sdggi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));

  sdggi->setZValue(-1);
  scene.addItem(sdggi);

  // Setup input handlers. They get events before the scene gets them
  // and the input they generate is passed to the triangulation with
  // the signal/slot mechanism
  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 0, true); // inputs polylines which are not closed
  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
		   this, SLOT(processInput(CGAL::Object)));

  //
  // Manual handling of actions
  //
  QObject::connect(this->actionQuit, SIGNAL(triggered()),
		   this, SLOT(close()));

  // We put mutually exclusive actions in an QActionGroup
  QActionGroup* ag = new QActionGroup(this);
  ag->addAction(this->actionInsertPolyline);

  // Check two actions
  this->actionInsertPolyline->setChecked(true);
  this->actionShowVoronoi->setChecked(true);
  this->actionShowConstraints->setChecked(true);

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Turn the vertical axis upside down
  this->graphicsView->scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Segment_voronoi_2.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
	  this, SLOT(open(QString)));

  if (has_file_argument) {
    open(file_to_open);
    statusBar()->showMessage(file_to_open, 0);
  }
}

void
MainWindow::processInput(CGAL::Object o)
{

  std::list<Point_2> points;
  if(CGAL::assign(points, o)){
    if(points.size() == 1) {
      svd.insert(points.front());
    }
    else {
      /*
      std::cout.precision(12);
      std::cout << points.size() << std::endl;
      for( std::list<Point_2>::iterator it =  points.begin(); it != points.end(); ++it){
	std::cout << *it << std::endl;
      }
      */
      insert_polyline(points.begin(), points.end());
    }
  }

  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  http://doc.trolltech.com/4.4/designer-using-a-component.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */
void
MainWindow::on_actionInsertPolyline_toggled(bool checked)
{
  if(checked){
    scene.installEventFilter(pi);
  } else {
    scene.removeEventFilter(pi);
  }
}

void
MainWindow::on_actionClear_triggered()
{
  svd.clear();
  Q_EMIT( changed());
}

void
MainWindow::open(QString fileName)
{
  if(! fileName.isEmpty()){
    if(fileName.endsWith(".polygons.cgal")){
      loadPolygonConstraints(fileName);
      this->addToRecentFiles(fileName);
    } else if(fileName.endsWith(".edg")){
      loadEdgConstraints(fileName);
      this->addToRecentFiles(fileName);
    } else if(fileName.endsWith(".pts")){
      loadPoints(fileName);
      this->addToRecentFiles(fileName);
    } else if(fileName.endsWith(".pin")){
      loadPointsInput(fileName);
      this->addToRecentFiles(fileName);
    } else if(fileName.endsWith(".cin")){
      loadSitesInput(fileName);
      this->addToRecentFiles(fileName);
    } else if(fileName.endsWith(".wkt", Qt::CaseInsensitive)){
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      loadWKT(fileName);
      this->addToRecentFiles(fileName);
#endif
    }
  }
}

void
MainWindow::on_actionLoadSegments_triggered()
{
  QString fileName =
    QFileDialog::getOpenFileName(this,
                                 tr("Open Constraint File"),
                                 ".",
                                 tr(
                                    "Cin  files (*.cin);;"
                                    "Pin  files (*.pin);;"
                                    "Pts  files (*.pts);;"
                                    "Edge files (*.edg);;"
                                    "Polylines files (*.polygons.cgal);;"
                                 #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                    "WKT files (*.WKT *.wkt)"
                                 #endif
                                                          ));
  open(fileName);
}

void
MainWindow::loadPolygonConstraints(QString fileName)
{
  K::Point_2 p,q, first;
  SVD::Vertex_handle vp, vq, vfirst;
  std::ifstream ifs(qPrintable(fileName));
  int n;
  while(ifs >> n){
    CGAL_assertion( n > 0 );
    ifs >> first;
    p = first;
    vfirst = vp = svd.insert(p);
    n--;
    while(n--){
      ifs >> q;
      vq = svd.insert(q, vp);
      svd.insert(vp,vq);
      p = q;
      vp = vq;
    }
    if (vfirst != vp) {
      svd.insert(vp, vfirst);
    }
  }

  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::loadEdgConstraints(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  CGAL::Timer tim;
  tim.start();
  std::ifstream ifs(qPrintable(fileName));
  bool first=true;
  int n;
  ifs >> n;

  K::Point_2 p,q, qold(0,0); // Initialize qold, as otherwise some g++ issue a unjustified warning

  SVD::Vertex_handle vp, vq, vqold;
  while(ifs >> p) {
    ifs >> q;
    if(p == q){
      continue;
    }
    if((!first) && (p == qold)){
      vp = vqold;
    } else {
      vp = svd.insert(p);
    }
    vq = svd.insert(q, vp);
    svd.insert(vp,vq);
    qold = q;
    vqold = vq;
    first = false;
  }

  tim.stop();
  statusBar()->showMessage(QString("Insertion took %1 seconds").arg(tim.time()), 2000);
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::loadPoints(QString fileName)
{
  K::Point_2 p;
  std::ifstream ifs(qPrintable(fileName));
  int n;
  while(ifs >> n){
    while(n--){
      ifs >> p;
      svd.insert(p);
    }
  }

  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::loadWKT(QString
                    #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                    fileName
                    #endif
                    )
{
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
  std::ifstream ifs(qPrintable(fileName));
  //Points
  do
    {
    std::vector<K::Point_2> mpts;
    CGAL::read_multi_point_WKT(ifs, mpts);
    for(const K::Point_2& p : mpts)
      svd.insert(p);
    }while(ifs.good() && !ifs.eof());
  //Lines
  ifs.clear();
  ifs.seekg(0, ifs.beg);
  do
    {
    typedef std::vector<K::Point_2> LineString;
    std::vector<LineString> mls;
    CGAL::read_multi_linestring_WKT(ifs, mls);
    for(const LineString& ls : mls)
    {
      if(ls.empty())
        continue;

      bool first=true;
      K::Point_2 p,q, qold(0,0); // Initialize qold, as otherwise some g++ issue a unjustified warning

      SVD::Vertex_handle vp, vq, vqold;
      LineString::const_iterator it = ls.begin();
      for(; it != ls.end(); ++it){
        p = *it++;
        q = *it;
        if(p == q){
          continue;
        }
        if((!first) && (p == qold)){
          vp = vqold;
        } else {
          vp = svd.insert(p);
        }
        vq = svd.insert(q, vp);
        svd.insert(vp,vq);
        qold = q;
        vqold = vq;
        first = false;
      }
    }
    }while(ifs.good() && !ifs.eof());

  //Polygons
  ifs.clear();
  ifs.seekg(0, ifs.beg);
  do
  {
    typedef CGAL::Polygon_with_holes_2<K> Polygon;
    std::vector<Polygon> mps;
    CGAL::read_multi_polygon_WKT(ifs, mps);
    for(const Polygon& poly : mps)
    {
      if(poly.outer_boundary().is_empty())
        continue;
      K::Point_2 p,q, first;
      SVD::Vertex_handle vp, vq, vfirst;
      Polygon::General_polygon_2::const_iterator it
          = poly.outer_boundary().begin();

        first = *it;
        p = first;
        vfirst = vp = svd.insert(p);
        for(; it != poly.outer_boundary().end(); ++it)
        {
          q = *it;
          vq = svd.insert(q, vp);
          if(vp != vq)
            svd.insert(vp,vq);
          p = q;
          vp = vq;
        }
        if (vfirst != vp) {
          svd.insert(vp, vfirst);
        }
      }
  }while(ifs.good() && !ifs.eof());

  Q_EMIT( changed());
  actionRecenter->trigger();
#endif
}

void
MainWindow::loadPointsInput(QString fileName)
{
  K::Point_2 p;
  std::ifstream ifs(qPrintable(fileName));
  while (ifs >> p) {
    svd.insert(p);
  }

  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::loadSitesInput(QString fileName)
{
  K::Point_2 p(0, 0);
  SVD::Site_2 site = SVD::Site_2::construct_site_2(p);
  std::ifstream ifs(qPrintable(fileName));
  while (ifs >> site) {
    svd.insert(site);

    CGAL_SDG_DEBUG( svd.file_output_verbose(std::cout); ) ;
  }

  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(sdggi->boundingRect());
  this->graphicsView->fitInView(sdggi->boundingRect(), Qt::KeepAspectRatio);
}

#include "Segment_voronoi_linf_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Segment Voronoi 2 demo");

  // Import resources from libCGAL (Qt5)
  CGAL_QT_INIT_RESOURCES;

  if (argc == 2) {
    has_file_argument = true;
    file_to_open = argv[1];
  } else {
    has_file_argument = false;
  }

  MainWindow mainWindow;
  mainWindow.show();

  return app.exec();
}


/*
 * dense_grid_visual.hpp
 *
 * Created on: Mar 28, 2018 21:02
 * Description:
 *
 * Reference:
 * [1]
 * https://www.learnopencv.com/applycolormap-for-pseudocoloring-in-opencv-c-python/
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#ifndef DENSE_GRID_VISUAL_HPP
#define DENSE_GRID_VISUAL_HPP

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "decomp/dense_grid.hpp"
#include "cvdraw/cvdraw.hpp"

namespace robotnav {
cv::Mat CreateColorMapFromEigenMatrix(const Eigen::MatrixXd &matrix,
                                      bool invert_y = false) {
  cv::Mat grey_img, color_img;

  // scale matrix to 0-255
  Eigen::MatrixXd scaled_matrix =
      (matrix - Eigen::MatrixXd::Ones(matrix.rows(), matrix.cols()) *
                    matrix.minCoeff()) /
      (matrix.maxCoeff() - matrix.minCoeff()) * 255.0;

  Eigen::MatrixXd inverted_matrix =
      Eigen::MatrixXd::Zero(matrix.rows(), matrix.cols());
  if (invert_y) {
    std::size_t row_num = matrix.rows();
    for (std::size_t i = 0; i < row_num; ++i)
      inverted_matrix.row(i) = scaled_matrix.row(row_num - i - 1);
    cv::eigen2cv(inverted_matrix, grey_img);
  } else {
    cv::eigen2cv(scaled_matrix, grey_img);
  }

  grey_img.convertTo(grey_img, CV_8U);
  cv::applyColorMap(grey_img, color_img, cv::COLORMAP_JET);

  return color_img;
}

void ShowMatrixAsImage(const Eigen::MatrixXd &matrix, std::string window_name,
                       bool save_img) {
  cv::Mat img;
  cv::eigen2cv(matrix, img);
  img.convertTo(img, CV_8UC3);
  CvIO::ShowImage(img, window_name, save_img);
}

void ShowMatrixAsColorMap(const Eigen::MatrixXd &matrix,
                          std::string window_name, bool save_img) {
  cv::Mat color_img = CreateColorMapFromEigenMatrix(matrix);
  CvIO::ShowImage(color_img, window_name, save_img);
}

void ShowPathOnMatrixAsColorMap(const Eigen::MatrixXd &matrix,
                                std::vector<RectGridIndex> waypoints,
                                std::string window_name, bool save_img) {
  cv::Mat color_img = CreateColorMapFromEigenMatrix(matrix);

  CvCanvas canvas(color_img);

  for (int i = 0; i < waypoints.size() - 1; ++i) {
    canvas.DrawLine({static_cast<double>(waypoints[i].GetX()),
                     static_cast<double>(waypoints[i].GetY())},
                    {static_cast<double>(waypoints[i + 1].GetX()),
                     static_cast<double>(waypoints[i + 1].GetY())},
                    cv::Scalar(244, 92, 66));
  }

  CvIO::ShowImage(color_img, window_name, save_img);
}
}  // namespace robotnav

#endif /* DENSE_GRID_VISUAL_HPP */


#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <crest/util/eigen_extensions.hpp>
#include <util/eigen_matchers.hpp>

#include <Eigen/Sparse>

using ::testing::Eq;

class sparse_submatrix_test : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        zero_10x10 = Eigen::SparseMatrix<int>(10, 10);

        diagonal_10x10 = Eigen::SparseMatrix<int>(10, 10);
        for (int i = 0; i < 10; ++i)
        {
            diagonal_10x10.insert(i, i) = i;
        }

        dense_5x5 = Eigen::SparseMatrix<int>(5, 5);
        for (int i = 0; i < 5; ++i)
        {
            for (int j = 0; j < 5; ++j)
            {
                dense_5x5.insert(i, j) = j + 5 * i;
            }
        }
    }

    Eigen::SparseMatrix<int> zero_10x10;
    Eigen::SparseMatrix<int> diagonal_10x10;
    Eigen::SparseMatrix<int> dense_5x5;
};

class submatrix_test : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        dense_5x5 << 0, 1, 2, 3, 4,
                5, 6, 7, 8, 9,
                10, 11, 12, 13, 14,
                15, 16, 17, 18, 19,
                20, 21, 22, 23, 24;
    }

    Eigen::Matrix<int, 5, 5> dense_5x5;
};

TEST_F(sparse_submatrix_test, zero_10x10)
{
    const auto rows = std::vector<int> { 3, 5, 7 };
    const auto cols = std::vector<int> { 0, 9 };
    const auto mat = sparse_submatrix(zero_10x10, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(3));
    EXPECT_THAT(mat.cols(), Eq(2));
    EXPECT_THAT(mat.nonZeros(), Eq(0));
}

TEST_F(sparse_submatrix_test, diagonal_10x10)
{
    const auto rows = std::vector<int> { 3, 5, 7 };
    const auto cols = std::vector<int> { 3, 4, 5 };
    const auto mat = sparse_submatrix(diagonal_10x10, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(3));
    EXPECT_THAT(mat.cols(), Eq(3));
    EXPECT_THAT(mat.nonZeros(), Eq(2));

    EXPECT_THAT(mat.coeff(0, 0), Eq(3));
    EXPECT_THAT(mat.coeff(1, 2), Eq(5));
}

TEST_F(sparse_submatrix_test, dense_5x5_arbitrary_indices)
{
    const auto rows = std::vector<int> { 1, 2, 4 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto mat = sparse_submatrix(dense_5x5, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(3));
    EXPECT_THAT(mat.cols(), Eq(2));
    EXPECT_THAT(mat.nonZeros(), Eq(6));

    Eigen::Matrix<int, 3, 2> expected;
    expected <<
             7, 9,
            12, 14,
            22, 24;

    EXPECT_TRUE(Eigen::MatrixXi(mat).isApprox(expected));
}

TEST_F(sparse_submatrix_test, dense_5x5_all_indices)
{
    const auto rows = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto cols = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto mat = sparse_submatrix(dense_5x5, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(5));
    EXPECT_THAT(mat.cols(), Eq(5));
    EXPECT_THAT(mat.nonZeros(), Eq(25));

    const Eigen::MatrixXi expected = Eigen::MatrixXi(dense_5x5);

    EXPECT_TRUE(Eigen::MatrixXi(mat).isApprox(expected));
}

TEST(sparse_submatrix_as_dense_test, dense_5x5_arbitrary_indices)
{
    auto dense_5x5 = Eigen::SparseMatrix<int>(5, 5);
    for (int i = 0; i < 5; ++i) {
        for (int j = 0; j < 5; ++j) {
            dense_5x5.insert(i, j) = j + 5 * i;
        }
    }

    const auto rows = std::vector<int> { 1, 2, 4 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto mat = sparse_submatrix_as_dense(dense_5x5, rows, cols);

    Eigen::Matrix<int, 3, 2> expected;
    expected <<
             7, 9,
            12, 14,
            22, 24;

    EXPECT_TRUE(mat == expected);
}

TEST_F(submatrix_test, dense_5x5)
{
    const auto rows = std::vector<int> { 1, 2, 4 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto mat = submatrix(dense_5x5, rows, cols);

    Eigen::Matrix<int, 3, 2> expected;
    expected <<
             7, 9,
            12, 14,
            22, 24;

    EXPECT_TRUE(mat.isApprox(expected));
}

TEST_F(submatrix_test, dense_5x5_all_indices)
{
    const auto rows = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto cols = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto mat = submatrix(dense_5x5, rows, cols);

    const Eigen::MatrixXi expected = Eigen::MatrixXi(dense_5x5);

    EXPECT_TRUE(mat.isApprox(expected));
}

TEST_F(sparse_submatrix_test, sparsity_pattern_zero_10x10) {
    const auto reverse_rows = std::vector<int> { -1, -1, -1, 0, -1, 1, -1, 2, -1, -1, -1};
    const auto cols = std::vector<int> { 0, 9 };
    const auto pattern = detail::submatrix_sparsity_pattern(zero_10x10, reverse_rows, cols);

    EXPECT_TRUE(pattern.isZero());
    EXPECT_THAT(pattern.rows(), Eq(2));
}

TEST_F(sparse_submatrix_test, sparsity_pattern_diagonal_10x10) {
    const auto reverse_rows = std::vector<int> { -1, -1, -1, 0, -1, 1, -1, 2, -1, -1, -1};
    const auto cols = std::vector<int> { 3, 4, 5 };
    const auto pattern = detail::submatrix_sparsity_pattern(diagonal_10x10, reverse_rows, cols);

    EXPECT_THAT(pattern.rows(), Eq(3));
    EXPECT_THAT(pattern(0), 1);
    EXPECT_THAT(pattern(1), 0);
    EXPECT_THAT(pattern(2), 1);
}

TEST_F(sparse_submatrix_test, sparsity_pattern_dense_5x5_arbitrary_indices)
{
    const auto reverse_rows = std::vector<int> { -1, 0, 0, -1, 2 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto pattern = detail::submatrix_sparsity_pattern(dense_5x5, reverse_rows, cols);

    EXPECT_THAT(pattern.rows(), Eq(2));
    EXPECT_THAT(pattern(0), 3);
    EXPECT_THAT(pattern(1), 3);
}

TEST_F(sparse_submatrix_test, sparsity_pattern_dense_5x5_all_indices)
{
    const auto reverse_rows = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto cols = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto pattern = detail::submatrix_sparsity_pattern(dense_5x5, reverse_rows, cols);

    EXPECT_THAT(pattern.rows(), Eq(5));
    EXPECT_THAT(pattern(0), Eq(5));
    EXPECT_THAT(pattern(1), Eq(5));
    EXPECT_THAT(pattern(2), Eq(5));
    EXPECT_THAT(pattern(3), Eq(5));
    EXPECT_THAT(pattern(4), Eq(5));
}


/**
 * \file waverage.cpp
 * \brief Compute average of spectra from FITS weighted by the SNR or the exposition time.
 * \author Audric Lemonnier
 * \version 0.0
 * \date 16/07/2020
 */

#include <iostream>
#include <fstream>

#include <cmath>

#include <string>
#include <sstream>
#include <iomanip>

#include <algorithm>
#include <vector>
#include <valarray>
#include <tuple>

#if __has_include (<filesystem>)
#include <filesystem>
#define FS_STD /**< std::filesystem availability (C++17) */
namespace fs = std::filesystem;
#elif __has_include (<experimental/filesystem>) && !__has_include (<filesystem>)
#include <experimental/filesystem>
#define FS_STDEXP /**< std::experimental::filesystem availability */
namespace fs = std::experimental::filesystem;
#elif __has_include(<boost/filesystem.hpp>) && !__has_include (<filesystem>) && !__has_include (<experimental/filesystem>)
#include <boost/filesystem.hpp>
#define FS_BOOST /**< boost::filesystem availability */
namespace fs = boost::filesystem;
#else
#error "No filesystem header found"
#endif

#include <waverage.hpp>

int main(int argc, char **argv) {

#ifdef HAS_BOOST_TIMER
    boost::timer::cpu_timer btTimer;
#endif

    hello();

    _io<> io;

    _io<>::notify("reading fits...", "", 3);
    io.read_fits_dir("./data/GIRAFFE_fits",".fits");
    io.set_WaveScale(10.);

    std::vector< std::valarray< std::valarray< double long > > > Spectra(io.get_valarrays());
    _op<> op(Spectra);

    _io<>::notify("removing zeros...", "", 3);
    op.remove_zero();

    _io<>::notify("resizing spectra...", "", 3);
    op.resize_spectr();

    _io<>::notify("rebuilding supports...", "", 3);
    op.rebuild_wlStep();

    _io<>::notify("filtering spectra...", "", 10);
//     op.filter_SG();

    op.remove_peaks(0);

    _io<>::notify("computing weighted mean...", "", 5);
    op.compute_wmean();

    _io<>::notify("writing data...", "", 3);
    op.write(io);
    op.write_mean("wmean.dat");

#ifdef HAS_BOOST_TIMER
    std::cout << "*" << btTimer.format()<< "\n";
#endif

    return EXIT_SUCCESS;
}


/**
 * @file clusterization.hpp
 * @brief Definition of the class "Clusterize".
 *
 * @author Maxim <john.jasper.doe@gmail.com>
 * @date 2020
 */

#ifndef CORE_CLUSTERIZATION_HPP_
#define CORE_CLUSTERIZATION_HPP_

#include <dlib/clustering.h>
#include <vector>

/** @brief The namespace of the "Clustering" project. */
namespace kkmeans {
/** @brief The namespace of the "Core" */
namespace core {

// Here we declare that our samples will be 2 dimensional column vectors.
// (Note that if you don't know the dimensionality of your vectors at compile time
// you can change the 2 to a 0 and then set the size at runtime)
using sample_t = dlib::matrix<double, 2, 1>;

/** @brief Input data type. */
using datas_t = std::vector<sample_t>;

/** @brief The Output data struct. */
struct clust_data {
  double x{.0};
  double y{.0};
  std::size_t clust{0};
};
using clust_datas_t = std::vector<core::clust_data>;

/** @brief The Clusterize class. */
class clusterize {
  // Now we are making a typedef for the kind of kernel we want to use.  I picked the
  // radial basis kernel because it only has one parameter and generally gives good
  // results without much fiddling.
  using kernel_type = dlib::radial_basis_kernel<sample_t>;

  datas_t input_;

public:
  /** @brief Constructor with params */
  explicit clusterize(datas_t&& input) noexcept;

  /** @brief Calculated */
  clust_datas_t exec(std::size_t k);
};

} /* core:: */
} /* kkmeans:: */

#endif /* CORE_CLUSTERIZATION_HPP_ */