//==============================================================================
/*
  ROTGEN - Runtime Overlay for Eigen
  Copyright : CODE RECKONS
  SPDX-License-Identifier: BSL-1.0
*/
//==============================================================================

#include <rotgen/rotgen.hpp>

#include "unit/common/references.hpp"
#include "unit/tests.hpp"
#include <Eigen/Eigen>

TTS_CASE_TPL("Test quaternion::angleAxis",
             float,
             double)<typename Scalar>(tts::type<Scalar>)
{
  // Eigen
  Eigen::Quaternion<Scalar> eigen_result;
  eigen_result =
    Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});

  // Rotgen
  rotgen::quaternion<Scalar> rotgen_result;
  rotgen_result =
    rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});

  TTS_EQUAL(rotgen_result.w(), eigen_result.w());
  TTS_EQUAL(rotgen_result.x(), eigen_result.x());
  TTS_EQUAL(rotgen_result.y(), eigen_result.y());
  TTS_EQUAL(rotgen_result.z(), eigen_result.z());
};

TTS_CASE_TPL("Test quaternion::operator* with rotgen::matrix",
             float,
             double)<typename Scalar>(tts::type<Scalar>)
{
  // Eigen
  Eigen::Quaternion<Scalar> eigen_quat;
  eigen_quat =
    Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});

  Eigen::Matrix<Scalar, 3, 1> eigen_other{3, 2, 1};
  auto eigen_result = eigen_quat * eigen_other;

  // Rotgen
  rotgen::quaternion<Scalar> rotgen_quat;
  rotgen_quat =
    rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});

  rotgen::matrix<Scalar, 3, 1> rotgen_other{3, 2, 1};
  auto rotgen_result = rotgen_quat * rotgen_other;

  for (auto i = 0; i < eigen_result.size(); i++)
    TTS_ULP_EQUAL(rotgen_result(i), eigen_result(i), 2);
};

TTS_CASE_TPL("Test quaternion::operator* with rotgen::map",
             float,
             double)<typename Scalar>(tts::type<Scalar>)
{
  Scalar data[] = {3, 2, 1};

  // Eigen
  Eigen::Quaternion<Scalar> eigen_quat;
  eigen_quat =
    Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});

  Eigen::Map<Eigen::Matrix<Scalar, 3, 1>> eigen_other{data};
  auto eigen_result = eigen_quat * eigen_other;

  // Rotgen
  rotgen::quaternion<Scalar> rotgen_quat;
  rotgen_quat =
    rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});

  rotgen::map<rotgen::matrix<Scalar, 3, 1>> rotgen_other{data};
  auto rotgen_result = rotgen_quat * rotgen_other;

  for (auto i = 0; i < eigen_result.size(); i++)
    TTS_ULP_EQUAL(rotgen_result(i), eigen_result(i), 2);
};

TTS_CASE_TPL("Test quaternion::operator* with rotgen::block",
             float,
             double)<typename Scalar>(tts::type<Scalar>)
{

  // Eigen
  Eigen::Quaternion<Scalar> eigen_quat;
  eigen_quat =
    Eigen::AngleAxis<Scalar>(13, Eigen::Matrix<Scalar, 3, 1>{1, 2, 3});

  Eigen::Matrix<Scalar, 16, 16> eigen_original_mat;
  rotgen::tests::prepare(16, 16, rotgen::tests::default_init_function,
                         eigen_original_mat);
  auto eigen_other = eigen_original_mat.template block<3, 1>(0, 0);
  auto eigen_result = eigen_quat * eigen_other;

  // Rotgen
  rotgen::quaternion<Scalar> rotgen_quat;
  rotgen_quat =
    rotgen::angleAxis<Scalar>(13, rotgen::matrix<Scalar, 3, 1>{1, 2, 3});

  rotgen::matrix<Scalar, 16, 16> rotgen_original_mat;
  rotgen::tests::prepare(16, 16, rotgen::tests::default_init_function,
                         rotgen_original_mat);
  auto rotgen_other = rotgen::extract<3, 1>(rotgen_original_mat, 0, 0);
  auto rotgen_result = rotgen_quat * rotgen_other;

  for (auto i = 0; i < eigen_result.size(); i++)
    TTS_ULP_EQUAL(rotgen_result(i), eigen_result(i), 2);
};