//
//  matrix_helper_test.cpp
//  matrix
//
//  Created by Sam Jaffe on 6/3/18.
//

#include <testing/xcode_gtest_helper.h>

#include "math/matrix/matrix_helpers.hpp"

#include "math/matrix/macro.h"

template <std::size_t N> using vec = math::vector::vector<double, N>;

namespace math { namespace matrix {
template <typename T, std::size_t R, std::size_t C>
void PrintTo(matrix<T, R, C> const & matr, std::ostream * out) {
  (*out) << "[\n";
  VECTOR_FOR_EACH_RANGE(i, R) {
    (*out) << "  [ ";
    VECTOR_FOR_EACH_RANGE(j, C) {
      if (j != 0) (*out) << ", ";
      (*out) << matr(i, j);
    }
    (*out) << " ]\n";
  }
  (*out) << "]";
}
}}

TEST(MatrixHelper, IdentityFunctionProducesOnes) {
  auto iden = math::matrix::identity<double, 4>();
  VECTOR_FOR_EACH_RANGE(i, 4) { EXPECT_THAT(iden.at(i, i), 1.0); }
  MATRIX_FOR_EACH_RANGE(i, 4, j, 4) {
    EXPECT_TRUE(i == j || iden.at(i, j) == 0);
  }
}

TEST(MatrixHelper, DiagonalWillFillWithAllZeros) {
  vec<4> const vector{1.5, 0.5, -1.0, 2.2};
  auto diag = math::matrix::diagonal(vector);
  VECTOR_FOR_EACH_RANGE(i, 4) { EXPECT_THAT(diag.at(i, i), vector.at(i)); }
  MATRIX_FOR_EACH_RANGE(i, 4, j, 4) {
    EXPECT_TRUE(i == j || diag.at(i, j) == 0);
  }
}

TEST(MatrixHelper, ScalarMatrixPiecewiseStretchesVector) {
  vec<3> const vector{1.5, 0.5, -1.0};
  vec<3> const x{2.0, 3.8, 11.0};
  auto const A = math::matrix::scalar(vector);
  EXPECT_THAT(A * x, vector * x);
}

TEST(MatrixHelper, TranslationMatrixAddsToEachElement) {
  vec<3> const vector{1.5, 0.5, -1.0};
  vec<3> const x{2.0, 3.8, 11.0};
  auto const A = math::matrix::translation(vector);
  EXPECT_THAT(A * x, vector + x);
}

using matr3 = math::matrix::matrix<double, 3, 3>;
using math::matrix::rotation;
using math::matrix::rotation_t;
namespace rotate = math::matrix::rotate;

class RotationTest
    : public ::testing::TestWithParam<std::tuple<rotation_t<3>, matr3>> {};

TEST(MatrixHelper, RotateByHalfPi) {
  auto const theta = M_PI / 2;
  auto const iden = math::matrix::identity<double, 2>();
  auto const expected = math::matrix::matrix<int, 2, 2>{{0, -1}, {1, 0}};

  auto result = iden * rotation(theta, rotate::ROT_2D);
  MATRIX_FOR_EACH_RANGE(i, 2, j, 2) {
    EXPECT_THAT(result(i, j),
                ::testing::DoubleNear(expected(i, j),
                                      std::numeric_limits<double>::epsilon()));
  }
}

TEST_P(RotationTest, RotateByHalfPi) {
  auto const theta = M_PI / 2;
  auto const iden = math::matrix::identity<double, 3>();
  auto const expected = std::get<1>(GetParam());

  auto result = iden * rotation(theta, std::get<0>(GetParam()));
  MATRIX_FOR_EACH_RANGE(i, 3, j, 3) {
    EXPECT_THAT(result(i, j),
                ::testing::DoubleNear(expected(i, j),
                                      std::numeric_limits<double>::epsilon()));
  }
}

TEST(MatrixHelper, RotateIntoLargerMatrixArea) {
  auto const theta = M_PI / 2;
  auto const iden = math::matrix::identity<double, 4>();
  auto const expected = math::matrix::matrix<double, 4, 4>{
      {0, -1, 0, 0}, {1, 0, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};

  auto result = iden * rotation<4>(theta, rotate::Z_AXIS);
  MATRIX_FOR_EACH_RANGE(i, 4, j, 4) {
    EXPECT_THAT(result(i, j),
                ::testing::DoubleNear(expected(i, j),
                                      std::numeric_limits<double>::epsilon()));
  }
}

std::vector<std::tuple<rotation_t<3>, matr3>> values{
    {rotate::X_AXIS, matr3{{1, 0, 0}, {0, 0, -1}, {0, 1, 0}}},
    {rotate::Y_AXIS, matr3{{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}},
    {rotate::Z_AXIS, matr3{{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}}};
INSTANTIATE_TEST_SUITE_P(MatrixHelper, RotationTest,
                         ::testing::ValuesIn(values));