/**
BSD 3-Clause License

Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <basalt/camera/generic_camera.hpp>
#include <basalt/camera/stereographic_param.hpp>

#include "gtest/gtest.h"
#include "test_utils.h"

template <typename CamT>
void testProjectJacobian() {
  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();

  using VecN = typename CamT::VecN;
  using Vec2 = typename CamT::Vec2;
  using Vec4 = typename CamT::Vec4;

  using Mat24 = typename CamT::Mat24;
  using Mat2N = typename CamT::Mat2N;

  for (const CamT &cam : test_cams) {
    for (int x = -10; x <= 10; x++) {
      for (int y = -10; y <= 10; y++) {
        for (int z = -1; z <= 5; z++) {
          Vec4 p(x, y, z, 1);

          Mat24 J_p;
          Mat2N J_param;

          Vec2 res1;

          bool success = cam.project(p, res1, &J_p, &J_param);

          if (success) {
            test_jacobian(
                "d_r_d_p", J_p,
                [&](const Vec4 &x) {
                  Vec2 res;
                  cam.project(p + x, res);
                  return res;
                },
                Vec4::Zero());

            test_jacobian(
                "d_r_d_param", J_param,
                [&](const VecN &x) {
                  Vec2 res;

                  CamT cam1 = cam;
                  cam1 += x;

                  cam1.project(p, res);
                  return res;
                },
                VecN::Zero());
          }
        }
      }
    }
  }
}

template <typename CamT>
void testProjectJacobian3() {
  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();

  constexpr int N = CamT::N;

  using Scalar = typename CamT::Scalar;
  using VecN = typename CamT::VecN;
  using Vec2 = typename CamT::Vec2;
  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;

  using Mat23 = Eigen::Matrix<Scalar, 2, 3, Eigen::RowMajor>;
  using Mat2N = Eigen::Matrix<Scalar, 2, N, Eigen::RowMajor>;

  for (const CamT &cam : test_cams) {
    for (int x = -10; x <= 10; x++) {
      for (int y = -10; y <= 10; y++) {
        for (int z = -1; z <= 5; z++) {
          Vec3 p_init(x, y, z);

          Scalar p_raw_array[3];
          Scalar res_raw_array[2];
          Scalar J_p_raw_array[2 * 3];
          Scalar J_param_raw_array[2 * N];

          Eigen::Map<Vec3> p(p_raw_array);
          p = p_init;

          Eigen::Map<Mat23> J_p(J_p_raw_array);
          Eigen::Map<Mat2N> J_param(J_param_raw_array);

          Eigen::Map<Vec2> res1(res_raw_array);

          bool success = cam.project(p, res1, &J_p, nullptr);
          success = cam.project(p, res1, nullptr, &J_param);

          if (success) {
            test_jacobian(
                "d_r_d_p", J_p,
                [&](const Vec3 &x) {
                  Vec2 res;
                  cam.project(p + x, res);
                  return res;
                },
                Vec3::Zero());

            test_jacobian(
                "d_r_d_param", J_param,
                [&](const VecN &x) {
                  Vec2 res;

                  CamT cam1 = cam;
                  cam1 += x;

                  cam1.project(p, res);
                  return res;
                },
                VecN::Zero());
          }
        }
      }
    }
  }
}

template <typename CamT>
void testProjectUnproject() {
  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();

  using Scalar = typename CamT::Vec2::Scalar;
  using Vec2 = typename CamT::Vec2;
  using Vec4 = typename CamT::Vec4;

  for (const CamT &cam : test_cams) {
    for (int x = -10; x <= 10; x++) {
      for (int y = -10; y <= 10; y++) {
        for (int z = 0; z <= 5; z++) {
          Vec4 p(x, y, z, 0.23424);

          Vec4 p_normalized = Vec4::Zero();
          p_normalized.template head<3>() = p.template head<3>().normalized();
          Vec2 res;
          bool success = cam.project(p, res);

          if (success) {
            Vec4 p_uproj;
            cam.unproject(res, p_uproj);

            EXPECT_TRUE(p_normalized.isApprox(
                p_uproj, Sophus::Constants<Scalar>::epsilonSqrt()))
                << "p_normalized " << p_normalized.transpose() << " p_uproj "
                << p_uproj.transpose();
          }
        }
      }
    }
  }
}

template <typename CamT>
void testUnprojectJacobians() {
  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();

  using VecN = typename CamT::VecN;
  using Vec2 = typename CamT::Vec2;
  using Vec4 = typename CamT::Vec4;

  using Mat42 = typename CamT::Mat42;
  using Mat4N = typename CamT::Mat4N;

  for (const CamT &cam : test_cams) {
    for (int x = -10; x <= 10; x++) {
      for (int y = -10; y <= 10; y++) {
        for (int z = 0; z <= 5; z++) {
          Vec4 p_3d(x, y, z, 0);

          Vec2 p;
          bool success = cam.project(p_3d, p);

          if (success) {
            Mat42 J_p;
            Mat4N J_param;

            Vec4 res1;
            cam.unproject(p, res1, &J_p, &J_param);

            test_jacobian(
                "d_r_d_p", J_p,
                [&](const Vec2 &x) {
                  Vec4 res = Vec4::Zero();
                  cam.unproject(p + x, res);
                  return res;
                },
                Vec2::Zero());

            test_jacobian(
                "d_r_d_param", J_param,
                [&](const VecN &x) {
                  Vec4 res = Vec4::Zero();
                  CamT cam1 = cam;
                  cam1 += x;

                  cam1.unproject(p, res);
                  return res;
                },
                VecN::Zero());
          }
        }
      }
    }
  }
}

template <typename CamT>
void testUnprojectJacobians3() {
  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();

  using Scalar = typename CamT::Scalar;
  constexpr int N = CamT::N;

  using VecN = typename CamT::VecN;
  using Vec2 = typename CamT::Vec2;
  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;

  using Mat32 = Eigen::Matrix<Scalar, 3, 2, Eigen::RowMajor>;
  using Mat3N = Eigen::Matrix<Scalar, 3, N, Eigen::RowMajor>;

  for (const CamT &cam : test_cams) {
    for (int x = -10; x <= 10; x++) {
      for (int y = -10; y <= 10; y++) {
        for (int z = 0; z <= 5; z++) {
          Vec3 p_3d(x, y, z);

          Vec2 p;
          bool success = cam.project(p_3d, p);

          if (success) {
            Mat32 J_p;
            Mat3N J_param;

            Vec3 res1;
            cam.unproject(p, res1, &J_p, nullptr);
            cam.unproject(p, res1, nullptr, &J_param);

            test_jacobian(
                "d_r_d_p", J_p,
                [&](const Vec2 &x) {
                  Vec3 res = Vec3::Zero();
                  cam.unproject(p + x, res);
                  return res;
                },
                Vec2::Zero());

            test_jacobian(
                "d_r_d_param", J_param,
                [&](const VecN &x) {
                  Vec3 res = Vec3::Zero();
                  CamT cam1 = cam;
                  cam1 += x;

                  cam1.unproject(p, res);
                  return res;
                },
                VecN::Zero());
          }
        }
      }
    }
  }
}

////////////////////////////////////////////////////////////////

TEST(CameraTestCase, PinholeProjectJacobians) {
  testProjectJacobian<basalt::PinholeCamera<double>>();
}
TEST(CameraTestCase, PinholeProjectJacobiansFloat) {
  testProjectJacobian<basalt::PinholeCamera<float>>();
}

TEST(CameraTestCase, UnifiedProjectJacobians) {
  testProjectJacobian<basalt::UnifiedCamera<double>>();
}
TEST(CameraTestCase, UnifiedProjectJacobiansFloat) {
  testProjectJacobian<basalt::UnifiedCamera<float>>();
}

TEST(CameraTestCase, ExtendedUnifiedProjectJacobians) {
  testProjectJacobian<basalt::ExtendedUnifiedCamera<double>>();
}
TEST(CameraTestCase, ExtendedUnifiedProjectJacobiansFloat) {
  testProjectJacobian<basalt::ExtendedUnifiedCamera<float>>();
}

TEST(CameraTestCase, KannalaBrandtProjectJacobians) {
  testProjectJacobian<basalt::KannalaBrandtCamera4<double>>();
}
TEST(CameraTestCase, KannalaBrandtProjectJacobiansFloat) {
  testProjectJacobian<basalt::KannalaBrandtCamera4<float>>();
}

TEST(CameraTestCase, DoubleSphereJacobians) {
  testProjectJacobian<basalt::DoubleSphereCamera<double>>();
}
TEST(CameraTestCase, FovCameraJacobians) {
  testProjectJacobian<basalt::FovCamera<double>>();
}

TEST(CameraTestCase, BalCameraJacobians) {
  testProjectJacobian<basalt::BalCamera<double>>();
}

TEST(CameraTestCase, BalCameraJacobiansFloat) {
  testProjectJacobian<basalt::BalCamera<float>>();
}

////////////////////////////////////////////////////////////////

TEST(CameraTestCase, PinholeProjectUnproject) {
  testProjectUnproject<basalt::PinholeCamera<double>>();
}
TEST(CameraTestCase, PinholeProjectUnprojectFloat) {
  testProjectUnproject<basalt::PinholeCamera<float>>();
}

TEST(CameraTestCase, UnifiedProjectUnproject) {
  testProjectUnproject<basalt::UnifiedCamera<double>>();
}
TEST(CameraTestCase, UnifiedProjectUnprojectFloat) {
  testProjectUnproject<basalt::UnifiedCamera<float>>();
}

TEST(CameraTestCase, ExtendedUnifiedProjectUnproject) {
  testProjectUnproject<basalt::ExtendedUnifiedCamera<double>>();
}
TEST(CameraTestCase, ExtendedUnifiedProjectUnprojectFloat) {
  testProjectUnproject<basalt::ExtendedUnifiedCamera<float>>();
}

TEST(CameraTestCase, KannalaBrandtProjectUnproject) {
  testProjectUnproject<basalt::KannalaBrandtCamera4<double>>();
}
TEST(CameraTestCase, KannalaBrandtProjectUnprojectFloat) {
  testProjectUnproject<basalt::KannalaBrandtCamera4<float>>();
}

TEST(CameraTestCase, DoubleSphereProjectUnproject) {
  testProjectUnproject<basalt::DoubleSphereCamera<double>>();
}
TEST(CameraTestCase, DoubleSphereProjectUnprojectFloat) {
  testProjectUnproject<basalt::DoubleSphereCamera<float>>();
}

TEST(CameraTestCase, FovProjectUnproject) {
  testProjectUnproject<basalt::FovCamera<double>>();
}

TEST(CameraTestCase, FovProjectUnprojectFloat) {
  testProjectUnproject<basalt::FovCamera<float>>();
}

TEST(CameraTestCase, BalProjectUnproject) {
  testProjectUnproject<basalt::BalCamera<double>>();
}

TEST(CameraTestCase, BalProjectUnprojectFloat) {
  testProjectUnproject<basalt::BalCamera<float>>();
}

/////////////////////////////////////////////////////////////////////////

TEST(CameraTestCase, PinholeUnprojectJacobians) {
  testUnprojectJacobians<basalt::PinholeCamera<double>>();
}
TEST(CameraTestCase, PinholeUnprojectJacobiansFloat) {
  testUnprojectJacobians<basalt::PinholeCamera<float>>();
}

TEST(CameraTestCase, UnifiedUnprojectJacobians) {
  testUnprojectJacobians<basalt::UnifiedCamera<double>>();
}
TEST(CameraTestCase, UnifiedUnprojectJacobiansFloat) {
  testUnprojectJacobians<basalt::UnifiedCamera<float>>();
}

TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobians) {
  testUnprojectJacobians<basalt::ExtendedUnifiedCamera<double>>();
}
TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobiansFloat) {
  testUnprojectJacobians<basalt::ExtendedUnifiedCamera<float>>();
}

TEST(CameraTestCase, KannalaBrandtUnprojectJacobians) {
  testUnprojectJacobians<basalt::KannalaBrandtCamera4<double>>();
}
// TEST(CameraTestCase, KannalaBrandtUnprojectJacobiansFloat) {
//  test_unproject_jacobians<basalt::KannalaBrandtCamera4<float>>();
//}

TEST(CameraTestCase, DoubleSphereUnprojectJacobians) {
  testUnprojectJacobians<basalt::DoubleSphereCamera<double>>();
}
// TEST(CameraTestCase, DoubleSphereUnprojectJacobiansFloat) {
//  test_unproject_jacobians<basalt::DoubleSphereCamera<float>>();
//}

TEST(CameraTestCase, FovUnprojectJacobians) {
  testUnprojectJacobians<basalt::FovCamera<double>>();
}
TEST(CameraTestCase, FovUnprojectJacobiansFloat) {
  testUnprojectJacobians<basalt::FovCamera<float>>();
}

////////////////////////////////////////////////////////////////

TEST(CameraTestCase, PinholeProjectJacobians3) {
  testProjectJacobian3<basalt::PinholeCamera<double>>();
}
TEST(CameraTestCase, PinholeProjectJacobiansFloat3) {
  testProjectJacobian3<basalt::PinholeCamera<float>>();
}

TEST(CameraTestCase, UnifiedProjectJacobians3) {
  testProjectJacobian3<basalt::UnifiedCamera<double>>();
}
TEST(CameraTestCase, UnifiedProjectJacobiansFloat3) {
  testProjectJacobian3<basalt::UnifiedCamera<float>>();
}

TEST(CameraTestCase, ExtendedUnifiedProjectJacobians3) {
  testProjectJacobian3<basalt::ExtendedUnifiedCamera<double>>();
}
TEST(CameraTestCase, ExtendedUnifiedProjectJacobiansFloat3) {
  testProjectJacobian3<basalt::ExtendedUnifiedCamera<float>>();
}

TEST(CameraTestCase, KannalaBrandtProjectJacobians3) {
  testProjectJacobian3<basalt::KannalaBrandtCamera4<double>>();
}
TEST(CameraTestCase, KannalaBrandtProjectJacobiansFloat3) {
  testProjectJacobian3<basalt::KannalaBrandtCamera4<float>>();
}

TEST(CameraTestCase, DoubleSphereJacobians3) {
  testProjectJacobian3<basalt::DoubleSphereCamera<double>>();
}
TEST(CameraTestCase, FovCameraJacobians3) {
  testProjectJacobian3<basalt::FovCamera<double>>();
}

TEST(CameraTestCase, BalCameraJacobians3) {
  testProjectJacobian3<basalt::BalCamera<double>>();
}

TEST(CameraTestCase, BalCameraJacobiansFloat3) {
  testProjectJacobian3<basalt::BalCamera<float>>();
}

////////////////////////////////////////////////////////////////

TEST(CameraTestCase, PinholeUnprojectJacobians3) {
  testUnprojectJacobians3<basalt::PinholeCamera<double>>();
}
TEST(CameraTestCase, PinholeUnprojectJacobiansFloat3) {
  testUnprojectJacobians3<basalt::PinholeCamera<float>>();
}

TEST(CameraTestCase, UnifiedUnprojectJacobians3) {
  testUnprojectJacobians3<basalt::UnifiedCamera<double>>();
}
TEST(CameraTestCase, UnifiedUnprojectJacobiansFloat3) {
  testUnprojectJacobians3<basalt::UnifiedCamera<float>>();
}

TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobians3) {
  testUnprojectJacobians3<basalt::ExtendedUnifiedCamera<double>>();
}
TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobiansFloat3) {
  testUnprojectJacobians3<basalt::ExtendedUnifiedCamera<float>>();
}

TEST(CameraTestCase, KannalaBrandtUnprojectJacobians3) {
  testUnprojectJacobians3<basalt::KannalaBrandtCamera4<double>>();
}
// TEST(CameraTestCase, KannalaBrandtUnprojectJacobiansFloat3) {
//  test_unproject_jacobians3<basalt::KannalaBrandtCamera4<float>>();
//}

TEST(CameraTestCase, DoubleSphereUnprojectJacobians3) {
  testUnprojectJacobians3<basalt::DoubleSphereCamera<double>>();
}
// TEST(CameraTestCase, DoubleSphereUnprojectJacobiansFloat3) {
//  test_unproject_jacobians3<basalt::DoubleSphereCamera<float>>();
//}

TEST(CameraTestCase, FovUnprojectJacobians3) {
  testUnprojectJacobians3<basalt::FovCamera<double>>();
}
TEST(CameraTestCase, FovUnprojectJacobiansFloat3) {
  testUnprojectJacobians3<basalt::FovCamera<float>>();
}

////////////////////////////////////////////////////////////////

template <typename CamT>
void testStereographicProjectJacobian() {
  using Vec2 = typename CamT::Vec2;
  using Vec4 = typename CamT::Vec4;
  using Mat24 = typename CamT::Mat24;

  for (int x = -10; x <= 10; x++) {
    for (int y = -10; y <= 10; y++) {
      Vec4 p(x, y, 5, 0.23424);

      Mat24 J_p;

      Vec2 res1 = CamT::project(p, &J_p);
      Vec2 res2 = CamT::project(p);

      ASSERT_TRUE(res1.isApprox(res2))
          << "res1 " << res1.transpose() << " res2 " << res2.transpose();

      test_jacobian(
          "d_r_d_p", J_p, [&](const Vec4 &x) { return CamT::project(p + x); },
          Vec4::Zero());
    }
  }
}

template <typename CamT>
void testStereographicProjectUnproject() {
  using Vec2 = typename CamT::Vec2;
  using Vec4 = typename CamT::Vec4;

  for (int x = -10; x <= 10; x++) {
    for (int y = -10; y <= 10; y++) {
      Vec4 p(x, y, 5, 0.23424);

      Vec4 p_normalized = Vec4::Zero();
      p_normalized.template head<3>() = p.template head<3>().normalized();
      Vec2 res = CamT::project(p);
      Vec4 p_uproj = CamT::unproject(res);

      ASSERT_TRUE(p_normalized.isApprox(p_uproj))
          << "p_normalized " << p_normalized.transpose() << " p_uproj "
          << p_uproj.transpose();
    }
  }
}

template <typename CamT>
void testStereographicUnprojectJacobian() {
  using Vec2 = typename CamT::Vec2;
  using Vec4 = typename CamT::Vec4;

  using Mat42 = typename CamT::Mat42;

  for (int x = -10; x <= 10; x++) {
    for (int y = -10; y <= 10; y++) {
      Vec4 p_3d(x, y, 5, 0.23424);

      Vec2 p = CamT::project(p_3d);

      Mat42 J_p;

      Vec4 res1 = CamT::unproject(p, &J_p);
      Vec4 res2 = CamT::unproject(p);

      ASSERT_TRUE(res1.isApprox(res2))
          << "res1 " << res1.transpose() << " res2 " << res2.transpose();

      test_jacobian(
          "d_r_d_p", J_p, [&](const Vec2 &x) { return CamT::unproject(p + x); },
          Vec2::Zero());
    }
  }
}

TEST(CameraTestCase, StereographicParamProjectJacobians) {
  testStereographicProjectJacobian<basalt::StereographicParam<double>>();
}
TEST(CameraTestCase, StereographicParamProjectJacobiansFloat) {
  testStereographicProjectJacobian<basalt::StereographicParam<float>>();
}

TEST(CameraTestCase, StereographicParamProjectUnproject) {
  testStereographicProjectUnproject<basalt::StereographicParam<double>>();
}
TEST(CameraTestCase, StereographicParamProjectUnprojectFloat) {
  testStereographicProjectUnproject<basalt::StereographicParam<float>>();
}

TEST(CameraTestCase, StereographicParamUnprojectJacobians) {
  testStereographicUnprojectJacobian<basalt::StereographicParam<double>>();
}
TEST(CameraTestCase, StereographicParamUnprojectJacobiansFloat) {
  testStereographicUnprojectJacobian<basalt::StereographicParam<float>>();
}

template <class Scalar, int N>
void testEvalOrReference() {
  using VecType = Eigen::Matrix<Scalar, N, 1>;
  using MapType = Eigen::Map<Eigen::Matrix<Scalar, N, 1>>;
  using MatType = Eigen::Matrix<Scalar, N, N>;

  Scalar raw_array[N];
  MapType mapped_p(raw_array);
  VecType p1, p2;
  MatType m1;
  p1.setRandom();
  p2.setRandom();
  m1.setRandom();
  mapped_p.setRandom();

  // Non-evaluated sum
  auto sum = p1 + p2;
  typename basalt::EvalOrReference<decltype(sum)>::Type res1(sum);
  static_assert(std::is_same_v<VecType, decltype(res1)>);

  // Non-evaluated operations with matrix
  auto affine = m1 * p1 + p2;
  typename basalt::EvalOrReference<decltype(affine)>::Type res2(affine);
  static_assert(std::is_same_v<VecType, decltype(res2)>);

  // Vector: Should be reference. No data copy.
  typename basalt::EvalOrReference<decltype(p1)>::Type res3(p1);
  static_assert(std::is_same_v<const VecType &, decltype(res3)>);
  ASSERT_EQ(&res3[0], &p1[0]);

  // Map: Should be reference. No data copy.
  typename basalt::EvalOrReference<decltype(mapped_p)>::Type res4(mapped_p);
  static_assert(
      std::is_same_v<const Eigen::MatrixBase<MapType> &, decltype(res4)>);
  ASSERT_EQ(&res4[0], &mapped_p[0]);

  // Map with standard Eigen eval: Copies data. Should not be used in the code.
  typename Eigen::internal::eval<MapType>::type res5(mapped_p);
  static_assert(std::is_same_v<VecType, decltype(res5)>);
  ASSERT_NE(&res5[0], &mapped_p[0]);
}

TEST(CameraTestCase, EvalOrReferenceTypeCast) {
  testEvalOrReference<double, 3>();
  testEvalOrReference<double, 4>();
  testEvalOrReference<float, 3>();
  testEvalOrReference<float, 4>();
}