#include <gtest/gtest.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "UKF/Types.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"
#include "comparisons.h"

enum MyFields {
    StaticPressure,
    DynamicPressure,
    Accelerometer,
    Gyroscope,
    Magnetometer
};

using MyMeasurementVector = UKF::DynamicMeasurementVector<
    UKF::Field<StaticPressure, real_t>,
    UKF::Field<DynamicPressure, real_t>,
    UKF::Field<Magnetometer, UKF::FieldVector>,
    UKF::Field<Accelerometer, UKF::Vector<3>>,
    UKF::Field<Gyroscope, UKF::Vector<3>>
>;

TEST(DynamicMeasurementVectorTest, Instantiation) {
    MyMeasurementVector test_measurement;

    EXPECT_EQ(11, MyMeasurementVector::MaxRowsAtCompileTime);
    EXPECT_EQ(11, test_measurement.max_size());
}

TEST(DynamicMeasurementVectorTest, Assignment) {
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));

    EXPECT_EQ(3, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_measurement.get_field<Gyroscope>());

    test_measurement.set_field<DynamicPressure>(4);

    EXPECT_EQ(4, test_measurement.size());
    EXPECT_EQ(4, test_measurement.get_field<DynamicPressure>());

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));

    EXPECT_EQ(7, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(5, 6, 7), test_measurement.get_field<Accelerometer>());

    test_measurement.set_field<StaticPressure>(8);

    EXPECT_EQ(8, test_measurement.size());
    EXPECT_EQ(8, test_measurement.get_field<StaticPressure>());

    test_measurement.set_field<Magnetometer>(UKF::FieldVector(9, 10, 11));

    EXPECT_EQ(11, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::FieldVector(9, 10, 11), test_measurement.get_field<Magnetometer>());

    UKF::Vector<11> expected;
    expected << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11;
    EXPECT_VECTOR_EQ(expected, test_measurement);
}

TEST(DynamicMeasurementVectorTest, Reassignment) {
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));

    EXPECT_EQ(3, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_measurement);
    EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_measurement.get_field<Gyroscope>());

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(4, 5, 6));

    EXPECT_EQ(3, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 5, 6), test_measurement);
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 5, 6), test_measurement.get_field<Gyroscope>());
}

TEST(DynamicMeasurementVectorTest, MultipleReassignment) {
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
    test_measurement.set_field<DynamicPressure>(4);
    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
    test_measurement.set_field<StaticPressure>(8);
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(9, 10, 11));

    EXPECT_EQ(11, test_measurement.size());
    UKF::Vector<11> expected;
    expected << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11;
    EXPECT_VECTOR_EQ(expected, test_measurement);

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(4, 5, 6));

    EXPECT_EQ(11, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 5, 6), test_measurement.get_field<Gyroscope>());
    expected << 4, 5, 6, 4, 5, 6, 7, 8, 9, 10, 11;
    EXPECT_VECTOR_EQ(expected, test_measurement);

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(7, 8, 9));

    EXPECT_EQ(11, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(7, 8, 9), test_measurement.get_field<Accelerometer>());
    expected << 4, 5, 6, 4, 7, 8, 9, 8, 9, 10, 11;
    EXPECT_VECTOR_EQ(expected, test_measurement);

    test_measurement.set_field<DynamicPressure>(1);

    EXPECT_EQ(11, test_measurement.size());
    EXPECT_EQ(1, test_measurement.get_field<DynamicPressure>());
    expected << 4, 5, 6, 1, 7, 8, 9, 8, 9, 10, 11;
    EXPECT_VECTOR_EQ(expected, test_measurement);

    test_measurement.set_field<StaticPressure>(3);

    EXPECT_EQ(11, test_measurement.size());
    EXPECT_EQ(3, test_measurement.get_field<StaticPressure>());
    expected << 4, 5, 6, 1, 7, 8, 9, 3, 9, 10, 11;
    EXPECT_VECTOR_EQ(expected, test_measurement);

    test_measurement.set_field<Magnetometer>(UKF::FieldVector(4, 5, 6));

    EXPECT_EQ(11, test_measurement.size());
    EXPECT_VECTOR_EQ(UKF::FieldVector(4, 5, 6), test_measurement.get_field<Magnetometer>());
    expected << 4, 5, 6, 1, 7, 8, 9, 3, 4, 5, 6;
    EXPECT_VECTOR_EQ(expected, test_measurement);
}

TEST(DynamicMeasurementVectorTest, CopyConstructor) {
    MyMeasurementVector test_measurement_1, test_measurement_2;

    test_measurement_1.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
    test_measurement_1.set_field<DynamicPressure>(4);
    test_measurement_1.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
    test_measurement_1.set_field<StaticPressure>(8);
    test_measurement_1.set_field<Magnetometer>(UKF::FieldVector(9, 10, 11));

    test_measurement_2 = test_measurement_1;

    EXPECT_EQ(test_measurement_1.size(), test_measurement_2.size());
    EXPECT_VECTOR_EQ(test_measurement_1, test_measurement_2);
    EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_measurement_2.get_field<Gyroscope>());
    EXPECT_EQ(4, test_measurement_2.get_field<DynamicPressure>());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(5, 6, 7), test_measurement_2.get_field<Accelerometer>());
    EXPECT_EQ(8, test_measurement_2.get_field<StaticPressure>());
    EXPECT_VECTOR_EQ(UKF::FieldVector(9, 10, 11), test_measurement_2.get_field<Magnetometer>());
}

enum MyStateVectorFields {
    AngularVelocity,
    Altitude,
    Velocity,
    Attitude
};

using MyStateVector = UKF::StateVector<
    UKF::Field<Velocity, UKF::Vector<3>>,
    UKF::Field<AngularVelocity, UKF::Vector<3>>,
    UKF::Field<Attitude, UKF::Quaternion>,
    UKF::Field<Altitude, real_t>
>;

namespace UKF {
/*
Define measurement model to be used in tests. NOTE: These are just for
testing, don't expect them to make any physical sense whatsoever.
*/
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Accelerometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8);
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Gyroscope>(const MyStateVector& state) {
    return state.get_field<AngularVelocity>();
}

template <> template <>
real_t MyMeasurementVector::expected_measurement
<MyStateVector, StaticPressure>(const MyStateVector& state) {
    return 101.3 - 1.2*(state.get_field<Altitude>() / 100.0);
}

template <> template <>
real_t MyMeasurementVector::expected_measurement
<MyStateVector, DynamicPressure>(const MyStateVector& state) {
    return 0.5 * 1.225 * state.get_field<Velocity>().squaredNorm();
}

template <> template <>
UKF::FieldVector MyMeasurementVector::expected_measurement
<MyStateVector, Magnetometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::FieldVector(0.45, 0, 0);
}

/*
These versions of the predicted measurement functions have non-state inputs.
This could be used to add predicted kinematic acceleration by feeding control
inputs into a dynamics model, for example.
*/
template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Accelerometer, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
    return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8) + input;
}

template <> template <>
UKF::Vector<3> MyMeasurementVector::expected_measurement
<MyStateVector, Gyroscope, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
    return state.get_field<AngularVelocity>();
}

template <> template <>
real_t MyMeasurementVector::expected_measurement
<MyStateVector, StaticPressure, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
    return 101.3 - 1.2*(state.get_field<Altitude>() / 100.0);
}

template <> template <>
real_t MyMeasurementVector::expected_measurement
<MyStateVector, DynamicPressure, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
    return 0.5 * 1.225 * state.get_field<Velocity>().squaredNorm();
}

template <> template <>
UKF::FieldVector MyMeasurementVector::expected_measurement
<MyStateVector, Magnetometer, UKF::Vector<3>>(const MyStateVector& state, const UKF::Vector<3>& input) {
    return state.get_field<Attitude>() * UKF::FieldVector(0.45, 0, 0) + input;
}

}

TEST(DynamicMeasurementVectorTest, SigmaPointGeneration) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> target_sigma_points(
        test_measurement.size(), MyStateVector::num_sigma());

    target_sigma_points <<  0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,
                            0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,
                         -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8,
                            1,      1,      1,      1,  4.606,      1,      1,      1,      1,      1,      1,      1,      1,      1, -2.606,      1,      1,      1,      1,      1,      1,
                            0,      0,      0,      0,      0,  3.606,      0,      0,      0,      0,      0,      0,      0,      0,      0, -3.606,      0,      0,      0,      0,      0,
                            0,      0,      0,      0,      0,      0,  3.606,      0,      0,      0,      0,      0,      0,      0,      0,      0, -3.606,      0,      0,      0,      0,
                         0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45, -0.238, -0.238,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45, -0.238, -0.238,   0.45,
                            0,      0,      0,      0,      0,      0,      0,      0,      0,  0.382,      0,      0,      0,      0,      0,      0,      0,      0,      0, -0.382,      0,
                            0,      0,      0,      0,      0,      0,      0,      0, -0.382,      0,      0,      0,      0,      0,      0,      0,      0,      0,  0.382,      0,      0,
                         89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3, 89.257,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3, 89.343,
                        8.575, 20.954, 25.371, 29.788,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575, 12.121,  7.704,  3.287,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575;
    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points;
    measurement_sigma_points = test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    EXPECT_VECTOR_EQ(target_sigma_points.col(0),  measurement_sigma_points.col(0));
    EXPECT_VECTOR_EQ(target_sigma_points.col(1),  measurement_sigma_points.col(1));
    EXPECT_VECTOR_EQ(target_sigma_points.col(2),  measurement_sigma_points.col(2));
    EXPECT_VECTOR_EQ(target_sigma_points.col(3),  measurement_sigma_points.col(3));
    EXPECT_VECTOR_EQ(target_sigma_points.col(4),  measurement_sigma_points.col(4));
    EXPECT_VECTOR_EQ(target_sigma_points.col(5),  measurement_sigma_points.col(5));
    EXPECT_VECTOR_EQ(target_sigma_points.col(6),  measurement_sigma_points.col(6));
    EXPECT_VECTOR_EQ(target_sigma_points.col(7),  measurement_sigma_points.col(7));
    EXPECT_VECTOR_EQ(target_sigma_points.col(8),  measurement_sigma_points.col(8));
    EXPECT_VECTOR_EQ(target_sigma_points.col(9),  measurement_sigma_points.col(9));
    EXPECT_VECTOR_EQ(target_sigma_points.col(10), measurement_sigma_points.col(10));
    EXPECT_VECTOR_EQ(target_sigma_points.col(11), measurement_sigma_points.col(11));
    EXPECT_VECTOR_EQ(target_sigma_points.col(12), measurement_sigma_points.col(12));
    EXPECT_VECTOR_EQ(target_sigma_points.col(13), measurement_sigma_points.col(13));
    EXPECT_VECTOR_EQ(target_sigma_points.col(14), measurement_sigma_points.col(14));
    EXPECT_VECTOR_EQ(target_sigma_points.col(15), measurement_sigma_points.col(15));
    EXPECT_VECTOR_EQ(target_sigma_points.col(16), measurement_sigma_points.col(16));
    EXPECT_VECTOR_EQ(target_sigma_points.col(17), measurement_sigma_points.col(17));
    EXPECT_VECTOR_EQ(target_sigma_points.col(18), measurement_sigma_points.col(18));
    EXPECT_VECTOR_EQ(target_sigma_points.col(19), measurement_sigma_points.col(19));
    EXPECT_VECTOR_EQ(target_sigma_points.col(20), measurement_sigma_points.col(20));
}

TEST(DynamicMeasurementVectorTest, SigmaPointGenerationWithInput) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> target_sigma_points(
        test_measurement.size(), MyStateVector::num_sigma());

    target_sigma_points <<  1,      1,      1,      1,      1,      1,      1,      1, -7.313,      1,      1,      1,      1,      1,      1,      1,      1,      1,  9.313,      1,      1,
                            2,      2,      2,      2,      2,      2,      2, 10.313,      2,      2,      2,      2,      2,      2,      2,      2,      2, -6.313,      2,      2,      2,
                         -6.8,   -6.8,   -6.8,   -6.8,   -6.8,   -6.8,   -6.8,  8.188,  8.188,   -6.8,   -6.8,   -6.8,   -6.8,   -6.8,   -6.8,   -6.8,   -6.8,  8.188,  8.188,   -6.8,   -6.8,
                            1,      1,      1,      1,  4.606,      1,      1,      1,      1,      1,      1,      1,      1,      1, -2.606,      1,      1,      1,      1,      1,      1,
                            0,      0,      0,      0,      0,  3.606,      0,      0,      0,      0,      0,      0,      0,      0,      0, -3.606,      0,      0,      0,      0,      0,
                            0,      0,      0,      0,      0,      0,  3.606,      0,      0,      0,      0,      0,      0,      0,      0,      0, -3.606,      0,      0,      0,      0,
                         1.45,   1.45,   1.45,   1.45,   1.45,   1.45,   1.45,   1.45,  0.762,  0.762,   1.45,   1.45,   1.45,   1.45,   1.45,   1.45,   1.45,   1.45,  0.762,  0.762,   1.45,
                            2,      2,      2,      2,      2,      2,      2,      2,      2,  2.382,      2,      2,      2,      2,      2,      2,      2,      2,      2,  1.618,      2,
                            3,      3,      3,      3,      3,      3,      3,      3,  2.618,      3,      3,      3,      3,      3,      3,      3,      3,      3,  3.382,      3,      3,
                         89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3, 89.257,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3, 89.343,
                        8.575, 20.954, 25.371, 29.788,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575, 12.121,  7.704,  3.287,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575;
    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points;
    measurement_sigma_points = test_measurement.calculate_sigma_point_distribution<MyStateVector>(
        sigma_points, UKF::Vector<3>(1, 2, 3));

    EXPECT_VECTOR_EQ(target_sigma_points.col(0),  measurement_sigma_points.col(0));
    EXPECT_VECTOR_EQ(target_sigma_points.col(1),  measurement_sigma_points.col(1));
    EXPECT_VECTOR_EQ(target_sigma_points.col(2),  measurement_sigma_points.col(2));
    EXPECT_VECTOR_EQ(target_sigma_points.col(3),  measurement_sigma_points.col(3));
    EXPECT_VECTOR_EQ(target_sigma_points.col(4),  measurement_sigma_points.col(4));
    EXPECT_VECTOR_EQ(target_sigma_points.col(5),  measurement_sigma_points.col(5));
    EXPECT_VECTOR_EQ(target_sigma_points.col(6),  measurement_sigma_points.col(6));
    EXPECT_VECTOR_EQ(target_sigma_points.col(7),  measurement_sigma_points.col(7));
    EXPECT_VECTOR_EQ(target_sigma_points.col(8),  measurement_sigma_points.col(8));
    EXPECT_VECTOR_EQ(target_sigma_points.col(9),  measurement_sigma_points.col(9));
    EXPECT_VECTOR_EQ(target_sigma_points.col(10), measurement_sigma_points.col(10));
    EXPECT_VECTOR_EQ(target_sigma_points.col(11), measurement_sigma_points.col(11));
    EXPECT_VECTOR_EQ(target_sigma_points.col(12), measurement_sigma_points.col(12));
    EXPECT_VECTOR_EQ(target_sigma_points.col(13), measurement_sigma_points.col(13));
    EXPECT_VECTOR_EQ(target_sigma_points.col(14), measurement_sigma_points.col(14));
    EXPECT_VECTOR_EQ(target_sigma_points.col(15), measurement_sigma_points.col(15));
    EXPECT_VECTOR_EQ(target_sigma_points.col(16), measurement_sigma_points.col(16));
    EXPECT_VECTOR_EQ(target_sigma_points.col(17), measurement_sigma_points.col(17));
    EXPECT_VECTOR_EQ(target_sigma_points.col(18), measurement_sigma_points.col(18));
    EXPECT_VECTOR_EQ(target_sigma_points.col(19), measurement_sigma_points.col(19));
    EXPECT_VECTOR_EQ(target_sigma_points.col(20), measurement_sigma_points.col(20));
}

TEST(DynamicMeasurementVectorTest, PartialSigmaPointGeneration) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> target_sigma_points(
        test_measurement.size(), MyStateVector::num_sigma());

    target_sigma_points <<  0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,
                            0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,
                         -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8;
    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points;
    measurement_sigma_points = test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    EXPECT_VECTOR_EQ(target_sigma_points.col(0),  measurement_sigma_points.col(0));
    EXPECT_VECTOR_EQ(target_sigma_points.col(1),  measurement_sigma_points.col(1));
    EXPECT_VECTOR_EQ(target_sigma_points.col(2),  measurement_sigma_points.col(2));
    EXPECT_VECTOR_EQ(target_sigma_points.col(3),  measurement_sigma_points.col(3));
    EXPECT_VECTOR_EQ(target_sigma_points.col(4),  measurement_sigma_points.col(4));
    EXPECT_VECTOR_EQ(target_sigma_points.col(5),  measurement_sigma_points.col(5));
    EXPECT_VECTOR_EQ(target_sigma_points.col(6),  measurement_sigma_points.col(6));
    EXPECT_VECTOR_EQ(target_sigma_points.col(7),  measurement_sigma_points.col(7));
    EXPECT_VECTOR_EQ(target_sigma_points.col(8),  measurement_sigma_points.col(8));
    EXPECT_VECTOR_EQ(target_sigma_points.col(9),  measurement_sigma_points.col(9));
    EXPECT_VECTOR_EQ(target_sigma_points.col(10), measurement_sigma_points.col(10));
    EXPECT_VECTOR_EQ(target_sigma_points.col(11), measurement_sigma_points.col(11));
    EXPECT_VECTOR_EQ(target_sigma_points.col(12), measurement_sigma_points.col(12));
    EXPECT_VECTOR_EQ(target_sigma_points.col(13), measurement_sigma_points.col(13));
    EXPECT_VECTOR_EQ(target_sigma_points.col(14), measurement_sigma_points.col(14));
    EXPECT_VECTOR_EQ(target_sigma_points.col(15), measurement_sigma_points.col(15));
    EXPECT_VECTOR_EQ(target_sigma_points.col(16), measurement_sigma_points.col(16));
    EXPECT_VECTOR_EQ(target_sigma_points.col(17), measurement_sigma_points.col(17));
    EXPECT_VECTOR_EQ(target_sigma_points.col(18), measurement_sigma_points.col(18));
    EXPECT_VECTOR_EQ(target_sigma_points.col(19), measurement_sigma_points.col(19));
    EXPECT_VECTOR_EQ(target_sigma_points.col(20), measurement_sigma_points.col(20));
}

TEST(DynamicMeasurementVectorTest, SigmaPointMean) {
    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points(
        MyMeasurementVector::max_size(), MyStateVector::num_sigma());
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    measurement_sigma_points <<  0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,
                                 0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,
                              -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8,
                                 1,      1,      1,      1,  4.606,      1,      1,      1,      1,      1,      1,      1,      1,      1, -2.606,      1,      1,      1,      1,      1,      1,
                                 0,      0,      0,      0,      0,  3.606,      0,      0,      0,      0,      0,      0,      0,      0,      0, -3.606,      0,      0,      0,      0,      0,
                                 0,      0,      0,      0,      0,      0,  3.606,      0,      0,      0,      0,      0,      0,      0,      0,      0, -3.606,      0,      0,      0,      0,
                              0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45, -0.238, -0.238,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45,   0.45, -0.238, -0.238,   0.45,
                                 0,      0,      0,      0,      0,      0,      0,      0,      0,  0.382,      0,      0,      0,      0,      0,      0,      0,      0,      0, -0.382,      0,
                                 0,      0,      0,      0,      0,      0,      0,      0, -0.382,      0,      0,      0,      0,      0,      0,      0,      0,      0,  0.382,      0,      0,
                              89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3, 89.257,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3,   89.3, 89.343,
                             8.575, 20.954, 25.371, 29.788,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575, 12.121,  7.704,  3.287,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575,  8.575;

    MyMeasurementVector expected_mean;

    expected_mean.set_field<Accelerometer>(UKF::Vector<3>(0.0, 0.0, -7.494));
    expected_mean.set_field<Gyroscope>(UKF::Vector<3>(1.0, 0.0, 0.0));
    expected_mean.set_field<Magnetometer>(UKF::FieldVector(0.45, 0, 0));
    expected_mean.set_field<StaticPressure>(89.3);
    expected_mean.set_field<DynamicPressure>(10.4125);

    EXPECT_VECTOR_EQ(expected_mean, test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points));
}

TEST(DynamicMeasurementVectorTest, PartialSigmaPointMean) {
    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points(
        3, MyStateVector::num_sigma());
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));

    measurement_sigma_points <<  0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,
                                 0,      0,      0,      0,      0,      0,      0,  8.314,      0,      0,      0,      0,      0,      0,      0,      0,      0, -8.314,      0,      0,      0,
                              -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,   -9.8,  5.188,  5.188,   -9.8,   -9.8;

    MyMeasurementVector expected_mean;

    expected_mean.set_field<Accelerometer>(UKF::Vector<3>(0.0, 0.0, -7.494));

    EXPECT_VECTOR_EQ(expected_mean, test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points));
}

TEST(DynamicMeasurementVectorTest, SigmaPointDeltas) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(1, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
        test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
    MyMeasurementVector::SigmaPointDeltas<MyStateVector> sigma_point_deltas(mean_measurement.size(), MyStateVector::num_sigma());
    MyMeasurementVector::SigmaPointDeltas<MyStateVector> target_sigma_point_deltas(mean_measurement.size(), MyStateVector::num_sigma());

    target_sigma_point_deltas <<       0,       0,       0,       0,       0,       0,       0,       0,  -8.314,       0,       0,       0,       0,       0,       0,       0,       0,       0,   8.314,       0,       0,
                                       0,       0,       0,       0,       0,       0,       0,   8.314,       0,       0,       0,       0,       0,       0,       0,       0,       0,  -8.314,       0,       0,       0,
                                  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  12.682,  12.682,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  12.682,  12.682,  -2.306,  -2.306,
                                       0,       0,       0,       0,   3.606,       0,       0,       0,       0,       0,       0,       0,       0,       0,  -3.606,       0,       0,       0,       0,       0,       0,
                                       0,       0,       0,       0,       0,   3.606,       0,       0,       0,       0,       0,       0,       0,       0,       0,  -3.606,       0,       0,       0,       0,       0,
                                       0,       0,       0,       0,       0,       0,   3.606,       0,       0,       0,       0,       0,       0,       0,       0,       0,  -3.606,       0,       0,       0,       0,
                                       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,
                                       0,       0,       0,       0,       0,       0,       0,       0,   3.606,       0,       0,       0,       0,       0,       0,       0,       0,       0,  -3.606,       0,       0,
                                       0,       0,       0,       0,       0,       0,       0,       0,       0,   3.606,       0,       0,       0,       0,       0,       0,       0,       0,       0,  -3.606,       0,
                                       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,  -0.043,       0,       0,       0,       0,       0,       0,       0,       0,       0,   0.043,
                                 -1.8375,  10.542,  14.959,  19.376, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375,  1.7082, -2.7086,  -7.126, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375;
    sigma_point_deltas = mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);

    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(0),  sigma_point_deltas.col(0));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(1),  sigma_point_deltas.col(1));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(2),  sigma_point_deltas.col(2));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(3),  sigma_point_deltas.col(3));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(4),  sigma_point_deltas.col(4));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(5),  sigma_point_deltas.col(5));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(6),  sigma_point_deltas.col(6));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(7),  sigma_point_deltas.col(7));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(8),  sigma_point_deltas.col(8));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(9),  sigma_point_deltas.col(9));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(10), sigma_point_deltas.col(10));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(11), sigma_point_deltas.col(11));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(12), sigma_point_deltas.col(12));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(13), sigma_point_deltas.col(13));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(14), sigma_point_deltas.col(14));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(15), sigma_point_deltas.col(15));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(16), sigma_point_deltas.col(16));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(17), sigma_point_deltas.col(17));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(18), sigma_point_deltas.col(18));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(19), sigma_point_deltas.col(19));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(20), sigma_point_deltas.col(20));
}

TEST(DynamicMeasurementVectorTest, PartialSigmaPointDeltas) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
        test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
    MyMeasurementVector::SigmaPointDeltas<MyStateVector> sigma_point_deltas(3, MyStateVector::num_sigma());
    MyMeasurementVector::SigmaPointDeltas<MyStateVector> target_sigma_point_deltas(3, MyStateVector::num_sigma());

    target_sigma_point_deltas <<       0,       0,       0,       0,       0,       0,       0,       0,  -8.314,       0,       0,      0,       0,       0,       0,       0,       0,       0,   8.314,       0,       0,
                                       0,       0,       0,       0,       0,       0,       0,   8.314,       0,       0,       0,      0,       0,       0,       0,       0,       0,  -8.314,       0,       0,       0,
                                  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  12.682,  12.682,  -2.306,  -2.306, -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  -2.306,  12.682,  12.682,  -2.306,  -2.306;
    sigma_point_deltas = mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);

    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(0),  sigma_point_deltas.col(0));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(1),  sigma_point_deltas.col(1));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(2),  sigma_point_deltas.col(2));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(3),  sigma_point_deltas.col(3));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(4),  sigma_point_deltas.col(4));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(5),  sigma_point_deltas.col(5));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(6),  sigma_point_deltas.col(6));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(7),  sigma_point_deltas.col(7));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(8),  sigma_point_deltas.col(8));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(9),  sigma_point_deltas.col(9));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(10), sigma_point_deltas.col(10));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(11), sigma_point_deltas.col(11));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(12), sigma_point_deltas.col(12));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(13), sigma_point_deltas.col(13));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(14), sigma_point_deltas.col(14));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(15), sigma_point_deltas.col(15));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(16), sigma_point_deltas.col(16));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(17), sigma_point_deltas.col(17));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(18), sigma_point_deltas.col(18));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(19), sigma_point_deltas.col(19));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(20), sigma_point_deltas.col(20));
}

TEST(DynamicMeasurementVectorTest, Innovation) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement, target_innovation;

    test_state.set_field<Velocity>(UKF::Vector<3>(10, 0, 0));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 1));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(1, 0, 0));
    test_measurement.set_field<DynamicPressure>(0.5 * 122.5);
    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 9.8));
    test_measurement.set_field<StaticPressure>(101.3);
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(0, 0.45, 0));

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1e-9, 1e-9, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
        test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);

    target_innovation.set_field<Gyroscope>(UKF::Vector<3>(1, 0, -1));
    target_innovation.set_field<DynamicPressure>(0);
    target_innovation.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 17.294));
    target_innovation.set_field<StaticPressure>(12.0);
    target_innovation.set_field<Magnetometer>(UKF::FieldVector(0, 0, 2));

    EXPECT_VECTOR_EQ(target_innovation, mean_measurement.calculate_innovation(test_measurement));
}

TEST(DynamicMeasurementVectorTest, PartialInnovation) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement, target_innovation;

    test_state.set_field<Velocity>(UKF::Vector<3>(10, 0, 0));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 1));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 9.8));

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1e-9, 1e-9, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
        test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);

    target_innovation.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 17.294));

    EXPECT_VECTOR_EQ(target_innovation, mean_measurement.calculate_innovation(test_measurement));
}

TEST(DynamicMeasurementVectorTest, SigmaPointCovariance) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(1, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
        test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
    MyMeasurementVector::SigmaPointDeltas<MyStateVector> sigma_point_deltas =
        mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);
    MyMeasurementVector::CovarianceMatrix calculated_covariance =
        mean_measurement.calculate_sigma_point_covariance<MyStateVector>(sigma_point_deltas);

    MyMeasurementVector::CovarianceMatrix expected_covariance(mean_measurement.size(), mean_measurement.size());

    expected_covariance << 5.31709,       0,       0,       0,       0,       0,       0, -2.3059,       0,       0,       0,
                                 0, 5.31709,       0,       0,       0,       0,       0,       0,       0,       0,       0,
                                 0,       0, 29.2440,       0,       0,       0,       0,       0,       0,       0,  -4.237,
                                 0,       0,       0,       1,       0,       0,       0,       0,       0,       0,       0,
                                 0,       0,       0,       0,       1,       0,       0,       0,       0,       0,       0,
                                 0,       0,       0,       0,       0,       1,       0,       0,       0,       0,       0,
                                 0,       0,       0,       0,       0,       0,       0,       0,       0,       0,       0,
                           -2.3059,       0,       0,       0,       0,       0,       0,       1,       0,       0,       0,
                                 0,       0,       0,       0,       0,       0,       0,       0,       1,       0,       0,
                                 0,       0,       0,       0,       0,       0,       0,       0,       0, 1.44e-4,       0,
                                 0,       0,  -4.237,       0,       0,       0,       0,       0,       0,       0,  32.263;

    EXPECT_VECTOR_EQ(expected_covariance.col(0),  calculated_covariance.col(0));
    EXPECT_VECTOR_EQ(expected_covariance.col(1),  calculated_covariance.col(1));
    EXPECT_VECTOR_EQ(expected_covariance.col(2),  calculated_covariance.col(2));
    EXPECT_VECTOR_EQ(expected_covariance.col(3),  calculated_covariance.col(3));
    EXPECT_VECTOR_EQ(expected_covariance.col(4),  calculated_covariance.col(4));
    EXPECT_VECTOR_EQ(expected_covariance.col(5),  calculated_covariance.col(5));
    EXPECT_VECTOR_EQ(expected_covariance.col(6),  calculated_covariance.col(6));
    EXPECT_VECTOR_EQ(expected_covariance.col(7),  calculated_covariance.col(7));
    EXPECT_VECTOR_EQ(expected_covariance.col(8),  calculated_covariance.col(8));
    EXPECT_VECTOR_EQ(expected_covariance.col(9),  calculated_covariance.col(9));
    EXPECT_VECTOR_EQ(expected_covariance.col(10),  calculated_covariance.col(10));
}

TEST(DynamicMeasurementVectorTest, PartialSigmaPointCovariance) {
    MyStateVector test_state;
    MyMeasurementVector test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
        test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);

    MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
    MyMeasurementVector::SigmaPointDeltas<MyStateVector> sigma_point_deltas =
        mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);
    MyMeasurementVector::CovarianceMatrix calculated_covariance =
        mean_measurement.calculate_sigma_point_covariance<MyStateVector>(sigma_point_deltas);

    MyMeasurementVector::CovarianceMatrix expected_covariance(mean_measurement.size(), mean_measurement.size());

    expected_covariance << 5.31709,       0,       0,
                                 0, 5.31709,       0,
                                 0,       0, 29.2440;

    EXPECT_VECTOR_EQ(expected_covariance.col(0),  calculated_covariance.col(0));
    EXPECT_VECTOR_EQ(expected_covariance.col(1),  calculated_covariance.col(1));
    EXPECT_VECTOR_EQ(expected_covariance.col(2),  calculated_covariance.col(2));
}

TEST(DynamicMeasurementVectorTest, MeasurementCovariance) {
    MyMeasurementVector test_measurement, expected_measurement;
    MyMeasurementVector::CovarianceVector measurement_covariance;

    measurement_covariance.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
    measurement_covariance.set_field<DynamicPressure>(4);
    measurement_covariance.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
    measurement_covariance.set_field<StaticPressure>(8);
    measurement_covariance.set_field<Magnetometer>(UKF::FieldVector(1, 2, 3));

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<DynamicPressure>(0);
    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    test_measurement.set_field<StaticPressure>(101.3);
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(-1, 0, 1));

    expected_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    expected_measurement.set_field<DynamicPressure>(0);
    expected_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    expected_measurement.set_field<StaticPressure>(101.3);
    expected_measurement.set_field<Magnetometer>(UKF::FieldVector(1, 1, 0));

    MyMeasurementVector::CovarianceMatrix expected_measurement_covariance = MyMeasurementVector::CovarianceMatrix::Zero(11, 11);
    expected_measurement_covariance.diagonal() << 1, 2, 3, 4, 5, 6, 7, 8, 0, 0, 0;
    expected_measurement_covariance.block<3, 3>(8, 8) <<  8, -8,  0,
                                                         -8,  8,  0,
                                                          0,  0, 16;

    MyMeasurementVector::CovarianceMatrix out_measurement_covariance =
        test_measurement.calculate_measurement_covariance(measurement_covariance, expected_measurement);

    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(0),  out_measurement_covariance.col(0));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(1),  out_measurement_covariance.col(1));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(2),  out_measurement_covariance.col(2));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(3),  out_measurement_covariance.col(3));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(4),  out_measurement_covariance.col(4));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(5),  out_measurement_covariance.col(5));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(6),  out_measurement_covariance.col(6));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(7),  out_measurement_covariance.col(7));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(8),  out_measurement_covariance.col(8));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(9),  out_measurement_covariance.col(9));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(10),  out_measurement_covariance.col(10));
}

TEST(DynamicMeasurementVectorTest, PartialMeasurementCovariance) {
    MyMeasurementVector test_measurement, expected_measurement;
    MyMeasurementVector::CovarianceVector measurement_covariance;

    measurement_covariance.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
    measurement_covariance.set_field<DynamicPressure>(4);
    measurement_covariance.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
    measurement_covariance.set_field<StaticPressure>(8);
    measurement_covariance.set_field<Magnetometer>(UKF::FieldVector(1, 2, 3));

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));

    expected_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));

    MyMeasurementVector::CovarianceMatrix expected_measurement_covariance = MyMeasurementVector::CovarianceMatrix::Zero(3, 3);
    expected_measurement_covariance.diagonal() << 1, 2, 3;

    MyMeasurementVector::CovarianceMatrix out_measurement_covariance =
        test_measurement.calculate_measurement_covariance(measurement_covariance, expected_measurement);

    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(0),  out_measurement_covariance.col(0));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(1),  out_measurement_covariance.col(1));
    EXPECT_VECTOR_EQ(expected_measurement_covariance.col(2),  out_measurement_covariance.col(2));
}

TEST(DynamicMeasurementVectorTest, MeasurementRootCovariance) {
    MyMeasurementVector test_measurement, expected_measurement;
    MyMeasurementVector::CovarianceVector measurement_root_covariance;

    measurement_root_covariance.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
    measurement_root_covariance.set_field<DynamicPressure>(4);
    measurement_root_covariance.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
    measurement_root_covariance.set_field<StaticPressure>(8);
    measurement_root_covariance.set_field<Magnetometer>(UKF::FieldVector(1, 2, 3));

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<DynamicPressure>(0);
    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<Magnetometer>(UKF::FieldVector(-1, 0, 1));

    expected_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    expected_measurement.set_field<DynamicPressure>(0);
    expected_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, -9.8));
    expected_measurement.set_field<StaticPressure>(101.3);
    expected_measurement.set_field<Magnetometer>(UKF::FieldVector(1, 1, 0));

    MyMeasurementVector::CovarianceMatrix expected_measurement_root_covariance = MyMeasurementVector::CovarianceMatrix::Zero(11, 11);
    expected_measurement_root_covariance.diagonal() << 1, 2, 3, 4, 5, 6, 7, 8, 0, 0, 0;
    expected_measurement_root_covariance.block<3, 3>(8, 8) <<  0, -4,  0,
                                                               0,  4,  0,
                                                              -2,  0, -6;

    MyMeasurementVector::CovarianceMatrix out_measurement_root_covariance =
        test_measurement.calculate_measurement_root_covariance(measurement_root_covariance, expected_measurement);

    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(0),  out_measurement_root_covariance.col(0));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(1),  out_measurement_root_covariance.col(1));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(2),  out_measurement_root_covariance.col(2));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(3),  out_measurement_root_covariance.col(3));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(4),  out_measurement_root_covariance.col(4));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(5),  out_measurement_root_covariance.col(5));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(6),  out_measurement_root_covariance.col(6));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(7),  out_measurement_root_covariance.col(7));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(8),  out_measurement_root_covariance.col(8));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(9),  out_measurement_root_covariance.col(9));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(10),  out_measurement_root_covariance.col(10));
}

TEST(DynamicMeasurementVectorTest, PartialMeasurementRootCovariance) {
    MyMeasurementVector test_measurement, expected_measurement;
    MyMeasurementVector::CovarianceVector measurement_root_covariance;

    measurement_root_covariance.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
    measurement_root_covariance.set_field<DynamicPressure>(4);
    measurement_root_covariance.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
    measurement_root_covariance.set_field<StaticPressure>(8);
    measurement_root_covariance.set_field<Magnetometer>(UKF::FieldVector(1, 2, 3));

    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));

    expected_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));

    MyMeasurementVector::CovarianceMatrix expected_measurement_root_covariance = MyMeasurementVector::CovarianceMatrix::Zero(3, 3);
    expected_measurement_root_covariance.diagonal() << 1, 2, 3;

    MyMeasurementVector::CovarianceMatrix out_measurement_root_covariance =
        test_measurement.calculate_measurement_root_covariance(measurement_root_covariance, expected_measurement);

    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(0),  out_measurement_root_covariance.col(0));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(1),  out_measurement_root_covariance.col(1));
    EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(2),  out_measurement_root_covariance.col(2));
}

TEST(DynamicMeasurementVectorTest, CalculateRotationVector) {
    UKF::Vector<3> test;

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(1, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(1, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(-1, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, -2/std::numeric_limits<real_t>::epsilon()), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 1, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, -2), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, -1, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 2), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 0, 1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 2, 0), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 0, -1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, -2, 0), test);

    /* Some random vectors, for the a = 0, f = 2 case. */
    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(
        UKF::Vector<3>(5.0746, -2.3911, 1.3564), UKF::Vector<3>(8.3439, -4.2832, 5.1440));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0.1070, 0.2438, 0.0294), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(
        UKF::Vector<3>(5.5833, 8.6802, -7.4019), UKF::Vector<3>(-8.4829, -8.9210, 0.6160));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4.4643, -4.3661, -1.7527), test);

    test = UKF::Detail::calculate_rotation_vector<MyMeasurementVector>(
        UKF::Vector<3>(2.0396, -4.7406, 3.0816), UKF::Vector<3>(-3.7757, 0.5707, -6.6870));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(-3.9209, -0.2624, 2.1914), test);
}

TEST(DynamicMeasurementVectorTest, CalculateRotationVectorJacobian) {
    UKF::Matrix<3, 3> test;

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(1, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 4/(std::numeric_limits<real_t>::epsilon()*std::numeric_limits<real_t>::epsilon()), 0).transpose(),
            (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 4/(std::numeric_limits<real_t>::epsilon()*std::numeric_limits<real_t>::epsilon())).transpose(),
            (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(1, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 1, 0).transpose(), (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 1).transpose(), (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(-1, 0, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 4/(std::numeric_limits<real_t>::epsilon()*std::numeric_limits<real_t>::epsilon()), 0).transpose(),
            (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 4/(std::numeric_limits<real_t>::epsilon()*std::numeric_limits<real_t>::epsilon())).transpose(),
            (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 1, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 4).transpose(), (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, -1, 0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 4).transpose(), (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 0, 1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 4, 0).transpose(), (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(UKF::Vector<3>(1, 0, 0), UKF::Vector<3>(0, 0, -1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 0, 0).transpose(), (test*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 4, 0).transpose(), (test*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(0, 0, 0).transpose(), (test*test.transpose()).row(2));

    /* Some random vectors, for the a = 0, f = 2 case. */
    Eigen::DiagonalMatrix<real_t, 3> c(UKF::Vector<3>(1, 2, 3));
    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(
        UKF::Vector<3>(5.0746, -2.3911, 1.3564), UKF::Vector<3>(8.3439, -4.2832, 5.1440));
    EXPECT_VECTOR_EQ(UKF::Vector<3>( 0.030105,  0.032038, -0.022155).transpose(), (test*c*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>( 0.032038,  0.073227,  0.009005).transpose(), (test*c*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(-0.022155,  0.009005,  0.043436).transpose(), (test*c*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(
        UKF::Vector<3>(5.5833, 8.6802, -7.4019), UKF::Vector<3>(-8.4829, -8.9210, 0.6160));
    EXPECT_VECTOR_EQ(UKF::Vector<3>( 0.790494, -0.776049, -0.353005).transpose(), (test*c*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(-0.776049,  0.768788,  0.446778).transpose(), (test*c*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(-0.353005,  0.446778,  1.609094).transpose(), (test*c*test.transpose()).row(2));

    test = UKF::Detail::calculate_rotation_vector_jacobian<MyMeasurementVector>(
        UKF::Vector<3>(2.0396, -4.7406, 3.0816), UKF::Vector<3>(-3.7757, 0.5707, -6.6870));
    EXPECT_VECTOR_EQ(UKF::Vector<3>( 1.850606, -0.474603, -1.085418).transpose(), (test*c*test.transpose()).row(0));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(-0.474603,  1.420474,  0.389206).transpose(), (test*c*test.transpose()).row(1));
    EXPECT_VECTOR_EQ(UKF::Vector<3>(-1.085418,  0.389206,  0.646079).transpose(), (test*c*test.transpose()).row(2));
}


#include "biome.h"

#include <iomanip>
#include <map>

#include <boost/log/trivial.hpp>

namespace WorldEngine
{

void BiomeSimulation(World& world)
{
   BOOST_LOG_TRIVIAL(info) << "Biome simulation start";

   uint32_t width  = world.width();
   uint32_t height = world.height();

   BiomeArrayType& biome = world.GetBiomeData();
   biome.resize(boost::extents[height][width]);

   std::map<Biome, uint32_t> biomeCounts;

   for (uint32_t y = 0; y < height; y++)
   {
      for (uint32_t x = 0; x < width; x++)
      {
         if (world.IsOcean(x, y))
         {
            biome[y][x] = Biome::Ocean;
         }
         else
         {
            TemperatureLevel temperature   = world.GetTemperatureLevel(x, y);
            HumidityLevel    humidityLevel = world.GetHumidityLevel(x, y);

            switch (temperature)
            {
            case TemperatureLevel::Polar:
               switch (humidityLevel)
               {
               case HumidityLevel::Superarid:
                  biome[y][x] = Biome::PolarDesert;
                  break;
               default: //
                  biome[y][x] = Biome::Ice;
                  break;
               }
               break;

            case TemperatureLevel::Alpine:
               switch (humidityLevel)
               {
               case HumidityLevel::Superarid:
                  biome[y][x] = Biome::SubpolarDryTundra;
                  break;
               case HumidityLevel::Perarid:
                  biome[y][x] = Biome::SubpolarMoistTundra;
                  break;
               case HumidityLevel::Arid:
                  biome[y][x] = Biome::SubpolarWetTundra;
                  break;
               default: //
                  biome[y][x] = Biome::SubpolarRainTundra;
                  break;
               }
               break;

            case TemperatureLevel::Boreal:
               switch (humidityLevel)
               {
               case HumidityLevel::Superarid:
                  biome[y][x] = Biome::BorealDesert;
                  break;
               case HumidityLevel::Perarid:
                  biome[y][x] = Biome::BorealDryScrub;
                  break;
               case HumidityLevel::Arid:
                  biome[y][x] = Biome::BorealMoistForest;
                  break;
               case HumidityLevel::Semiarid:
                  biome[y][x] = Biome::BorealWetForest;
                  break;
               default: //
                  biome[y][x] = Biome::BorealRainForest;
                  break;
               }
               break;

            case TemperatureLevel::Cool:
               switch (humidityLevel)
               {
               case HumidityLevel::Superarid:
                  biome[y][x] = Biome::CoolTemperateDesert;
                  break;
               case HumidityLevel::Perarid:
                  biome[y][x] = Biome::CoolTemperateDesertScrub;
                  break;
               case HumidityLevel::Arid:
                  biome[y][x] = Biome::CoolTemperateSteppe;
                  break;
               case HumidityLevel::Semiarid:
                  biome[y][x] = Biome::CoolTemperateMoistForest;
                  break;
               case HumidityLevel::Subhumid:
                  biome[y][x] = Biome::CoolTemperateWetForest;
                  break;
               default: //
                  biome[y][x] = Biome::CoolTemperateRainForest;
                  break;
               }
               break;

            case TemperatureLevel::Warm:
               switch (humidityLevel)
               {
               case HumidityLevel::Superarid:
                  biome[y][x] = Biome::WarmTemperateDesert;
                  break;
               case HumidityLevel::Perarid:
                  biome[y][x] = Biome::WarmTemperateDesertScrub;
                  break;
               case HumidityLevel::Arid:
                  biome[y][x] = Biome::WarmTemperateThornScrub;
                  break;
               case HumidityLevel::Semiarid:
                  biome[y][x] = Biome::WarmTemperateDryForest;
                  break;
               case HumidityLevel::Subhumid:
                  biome[y][x] = Biome::WarmTemperateMoistForest;
                  break;
               case HumidityLevel::Humid:
                  biome[y][x] = Biome::WarmTemperateWetForest;
                  break;
               default: //
                  biome[y][x] = Biome::WarmTemperateRainForest;
                  break;
               }
               break;

            case TemperatureLevel::Subtropical:
               switch (humidityLevel)
               {
               case HumidityLevel::Superarid:
                  biome[y][x] = Biome::SubtropicalDesert;
                  break;
               case HumidityLevel::Perarid:
                  biome[y][x] = Biome::SubtropicalDesertScrub;
                  break;
               case HumidityLevel::Arid:
                  biome[y][x] = Biome::SubtropicalThornWoodland;
                  break;
               case HumidityLevel::Semiarid:
                  biome[y][x] = Biome::SubtropicalDryForest;
                  break;
               case HumidityLevel::Subhumid:
                  biome[y][x] = Biome::SubtropicalMoistForest;
                  break;
               case HumidityLevel::Humid:
                  biome[y][x] = Biome::SubtropicalWetForest;
                  break;
               default: //
                  biome[y][x] = Biome::SubtropicalRainForest;
                  break;
               }
               break;

            case TemperatureLevel::Tropical:
               switch (humidityLevel)
               {
               case HumidityLevel::Superarid:
                  biome[y][x] = Biome::TropicalDesert;
                  break;
               case HumidityLevel::Perarid:
                  biome[y][x] = Biome::TropicalDesertScrub;
                  break;
               case HumidityLevel::Arid:
                  biome[y][x] = Biome::TropicalThornWoodland;
                  break;
               case HumidityLevel::Semiarid:
                  biome[y][x] = Biome::TropicalVeryDryForest;
                  break;
               case HumidityLevel::Subhumid:
                  biome[y][x] = Biome::TropicalDryForest;
                  break;
               case HumidityLevel::Humid:
                  biome[y][x] = Biome::TropicalMoistForest;
                  break;
               case HumidityLevel::Perhumid:
                  biome[y][x] = Biome::TropicalWetForest;
                  break;
               default: //
                  biome[y][x] = Biome::TropicalRainForest;
                  break;
               }
               break;

            default: // Invalid
               biome[y][x] = Biome::BareRock;
               break;
            }
         }

         biomeCounts[biome[y][x]]++;
      }
   }

   BOOST_LOG_TRIVIAL(debug) << "Biome obtained:";
   for (auto const& bc : biomeCounts)
   {
      BOOST_LOG_TRIVIAL(debug)
         << "  " << std::left << std::setw(30) << std::setfill(' ') << bc.first
         << ": " << std::right << std::setw(7) << std::setfill(' ')
         << bc.second;
   }

   BOOST_LOG_TRIVIAL(info) << "Biome simulation finish";
}

} // namespace WorldEngine


/*
	Copyright (C) 2013-2016 by Kristina Simpson <sweet.kristas@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgement in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#include <iomanip>
#include <boost/algorithm/string.hpp>

#include "hex_helper.hpp"
#include "hex_loader.hpp"
#include "hex_map.hpp"
#include "tile_rules.hpp"

#include "random.hpp"
#include "unit_test.hpp"

namespace
{
	std::string rot_replace(const std::string& str, const std::vector<std::string>& rotations, int rot)
	{
		//if(rot == 0) {
		//	return str;
		//}
		auto pos = str.find("@R");
		if(pos == std::string::npos) {
			return str;
		}

		std::string res;
		bool done = false;
		auto start_pos = 0;
		while(pos != std::string::npos) {
			res += str.substr(start_pos, pos - start_pos);
			int index = str[pos + 2] - '0';
			ASSERT_LOG(index >= 0 && index <= 5, "Invalid @R value: " << index);
			res += rotations[(index + rot)%rotations.size()];
			start_pos = pos + 3;
			pos = str.find("@R", start_pos);
		}
		res += str.substr(start_pos);
		//LOG_DEBUG("replaced " << str << " with " << res);
		return res;
	}

	// n*60 is the degrees of rotation
	// c is the center point
	// p is the co-ordinate to rotate.
	point rotate_point(int n, const point& c, const point& p)
	{
		point res = p;
		int x_p, y_p, z_p;
		int x_c, y_c, z_c;
		if(n > 0) {
			hex::evenq_to_cube_coords(p, &x_p, &y_p, &z_p);
			hex::evenq_to_cube_coords(c, &x_c, &y_c, &z_c);

			const int p_from_c_x = x_p - x_c;
			const int p_from_c_y = y_p - y_c;
			const int p_from_c_z = z_p - z_c;

			int r_from_c_x = p_from_c_x,
				r_from_c_y = p_from_c_y,
				r_from_c_z = p_from_c_z;
			for(int i = 0; i != n; ++i) {
				const int x = r_from_c_x, y = r_from_c_y, z = r_from_c_z;
				r_from_c_x = -z;
				r_from_c_y = -x;
				r_from_c_z = -y;
			}
			const int r_x = r_from_c_x + x_c;
			const int r_y = r_from_c_y + y_c;
			const int r_z = r_from_c_z + z_c;
			res = hex::cube_to_evenq_coords(r_x, r_y, r_z);
		}
		return res;
	}

	bool string_match(const std::string& s1, const std::string& s2)
	{
		std::string::const_iterator s1it = s1.cbegin();
		std::string::const_iterator s2it = s2.cbegin();
		while(s1it != s1.cend() && s2it != s2.cend()) {
			if(*s1it == '*') {
				++s1it;
				if(s1it == s1.cend()) {
					// an asterisk at the end of the string matches all, so just return a match.
					return true;
				}
				while(s2it != s2.cend() && *s2it != *s1it) {
					++s2it;
				}
				if(s2it == s2.cend() && s1it != s1.cend()) {
					return false;
				}
				++s1it;
				++s2it;
			} else {
				if(*s1it++ != *s2it++) {
					return false;
				}
			}
		}
		if(s1it != s1.cend() || s2it != s2.cend()) {
			return false;
		}
		return true;
	}

	point add_hex_coord(const point& p1, const point& p2)
	{
		int x_p1, y_p1, z_p1;
		int x_p2, y_p2, z_p2;
		hex::evenq_to_cube_coords(p1, &x_p1, &y_p1, &z_p1);
		hex::evenq_to_cube_coords(p2, &x_p2, &y_p2, &z_p2);
		return hex::cube_to_evenq_coords(x_p1 + x_p2, y_p1 + y_p2, z_p1 + z_p2);
	}

	point sub_hex_coord(const point& p1, const point& p2)
	{
		int x_p1, y_p1, z_p1;
		int x_p2, y_p2, z_p2;
		hex::evenq_to_cube_coords(p1, &x_p1, &y_p1, &z_p1);
		hex::evenq_to_cube_coords(p2, &x_p2, &y_p2, &z_p2);
		return hex::cube_to_evenq_coords(x_p1 - x_p2, y_p1 - y_p2, z_p1 - z_p2);
	}

	point center_point(const point& from_center, const point& to_center, const point& p)
	{
		int x_p, y_p, z_p;
	    int x_c, y_c, z_c;
        hex::evenq_to_cube_coords(p, &x_p, &y_p, &z_p);
        hex::evenq_to_cube_coords(from_center, &x_c, &y_c, &z_c);

        const int p_from_c_x = x_p - x_c;
        const int p_from_c_y = y_p - y_c;
        const int p_from_c_z = z_p - z_c;

		int x_r, y_r, z_r;
		hex::evenq_to_cube_coords(to_center, &x_r, &y_r, &z_r);
		return hex::cube_to_evenq_coords(x_r + p_from_c_x, y_r + p_from_c_y, z_r + p_from_c_z);
	}

	point pixel_distance(const point& from, const point& to, int hex_size)
	{
		auto f = hex::get_pixel_pos_from_tile_pos_evenq(from, hex_size);
		auto t = hex::get_pixel_pos_from_tile_pos_evenq(to, hex_size);
		return t - f;
	}
}

namespace hex
{
	TerrainRule::TerrainRule(const variant& v)
		: absolute_position_(nullptr),
		  mod_position_(nullptr),
		  rotations_(),
		  set_flag_(),
		  no_flag_(),
		  has_flag_(),
		  map_(),
		  center_(),
		  tile_data_(),
		  image_(),
		  pos_offset_(),
		  probability_(v["probability"].as_int32(100))
	{
		if(v.has_key("x")) {
			absolute_position_ = std::unique_ptr<point>(new point(v["x"].as_int32()));
		}
		if(v.has_key("y")) {
			if(absolute_position_ != nullptr) {
				absolute_position_->y = v["y"].as_int32();
			} else {
				absolute_position_ = std::unique_ptr<point>(new point(0, v["y"].as_int32()));
			}
		}
		if(v.has_key("mod_x")) {
			mod_position_ = std::unique_ptr<point>(new point(v["mod_x"].as_int32()));
		}
		if(v.has_key("mod_y")) {
			if(mod_position_ != nullptr) {
				mod_position_->y = v["mod_y"].as_int32();
			} else {
				mod_position_ = std::unique_ptr<point>(new point(0, v["mod_y"].as_int32()));
			}
		}
		if(v.has_key("rotations")) {
			rotations_ = v["rotations"].as_list_string();
		}
		std::vector<std::string> set_no_flag;
		if(v.has_key("set_no_flag")) {
			set_no_flag = v["set_no_flag"].as_list_string();
		}
		if(v.has_key("set_flag")) {
			set_flag_ = v["set_flag"].as_list_string();
		}
		for(const auto& snf : set_no_flag) {
			set_flag_.emplace_back(snf);
		}
		if(v.has_key("no_flag")) {
			no_flag_ = v["no_flag"].as_list_string();
		}
		for(const auto& snf : set_no_flag) {
			no_flag_.emplace_back(snf);
		}
		if(v.has_key("has_flag")) {
			has_flag_ = v["has_flag"].as_list_string();
		}
		if(v.has_key("map")) {
			map_ = v["map"].as_list_string();
		}

		if(v.has_key("image")) {
			const auto& img_v = v["image"];
			if(img_v.is_list()) {
				for(const auto& img : img_v.as_list()) {
					image_.emplace_back(new TileImage(img));
				}
			} else if(v.is_map()) {
				image_.emplace_back(new TileImage(v["image"]));
			}
		}
	}

	std::string TerrainRule::toString() const
	{
		std::stringstream ss;
		if(absolute_position_) {
			ss << "x,y: " << *absolute_position_ << "; ";
		}
		if(mod_position_) {
			ss << "mod_x/y: " << *mod_position_ << "; ";
		}
		if(!rotations_.empty()) {
			ss << "rotations:";
			for(const auto& rot : rotations_) {
				ss << " " << rot;
			}
			ss << "; ";
		}
		if(!image_.empty()) {
			ss << "images: ";
			for(const auto& img : image_) {
				ss << " " << img->toString();
			}
			ss << "; ";
		}
		if(!tile_data_.empty()) {
			ss << "tiles: ";
			for(const auto& td : tile_data_) {
				ss << " " << td->toString();
			}
			ss << "; ";
		}

		return ss.str();
	}

	void TerrainRule::preProcessMap(const variant& tiles)
	{
		if(!tiles.is_null()) {
			if(tiles.is_list()) {
				for(const auto& tile : tiles.as_list()) {
					auto td = std::unique_ptr<TileRule>(new TileRule(shared_from_this(), tile));
					tile_data_.emplace_back(std::move(td));
				}
			} else if(tiles.is_map()) {
				tile_data_.emplace_back(std::unique_ptr<TileRule>(new TileRule(shared_from_this(), tiles)));
			} else {
				ASSERT_LOG(false, "Tile data was neither list or map.");
			}
		}

		// Map processing.
		if(map_.empty()) {
			return;
		}
		std::string first_line = boost::trim_copy(map_.front());
		const bool odd_start = first_line.front() == ',';
		int lineno = odd_start ? 0 : 1;
		auto td = std::unique_ptr<TileRule>(new TileRule(shared_from_this()));
		std::vector<point> coord_list;
		for(const auto& map_line : map_) {
			std::vector<std::string> strs;
			std::string ml = boost::erase_all_copy(boost::erase_all_copy(map_line, "\t"), " ");
			boost::split(strs, ml, boost::is_any_of(","));
			// valid symbols are asterisk(*), period(.) and tile references(0-9).
			// '.' means this rule does not apply to this hex
			// '*' means this rule applies to this hex, but this hex can be any terrain type
			// an empty string is an odd line.
			int colno = 0;
			for(auto& str : strs) {
				const int x = colno * 2 - ((lineno + 1) % 2);
				const int y = lineno / 2;
				if(str == ".") {
					coord_list.emplace_back(x, y);
				} else if(str.empty()) {
					// ignore
				} else if(str == "*") {
					td->addPosition(point(x, y));
					coord_list.emplace_back(x, y);
				} else {
					coord_list.emplace_back(x, y);
					try {
						int pos = boost::lexical_cast<int>(str);
						bool found = false;
						for(auto& td : tile_data_) {
							if(td->getMapPos() == pos) {
								td->addPosition(point(x, y));
								if(pos == 1) {
									center_ = point(x, y);
								}
								found = true;
							}
						}
						ASSERT_LOG(found, "No tile for pos: " << pos);
					} catch(boost::bad_lexical_cast&) {
						ASSERT_LOG(false, "Unable to convert to number" << str);
					}
				}
				if(!((lineno % 2) != 0 && strs.front().empty())) {
					++colno;
				}
			}
			++lineno;
		}

		// Code to calculate the offset needed when an image is specified in the base terrain_graphics element.
		if(!image_.empty()) {
			const int max_loops = rotations_.empty() ? 1 : rotations_.size();
			pos_offset_.resize(max_loops);
			if(odd_start) {
				for(int rot = 0; rot != max_loops; ++rot) {
					pos_offset_[rot] = point(0, -g_hex_tile_size);
				}
			} else {
				if(rotations_.empty()) {
					pos_offset_[0] = point();
				} else {
					for(int rot = 0; rot != max_loops; ++rot) {
						point min_coord(std::numeric_limits<int>::max(), std::numeric_limits<int>::max());
						for(const auto& p : coord_list) {
							const point rotated_p = rotate_point(rot, center_, p);
							if(rotated_p.x <= min_coord.x) {
								if(!(rotated_p.x == min_coord.x && rotated_p.y > min_coord.y)) {
									min_coord = rotated_p;
								}
							}
						}
						if(rot % 2) {
							// odd needs offsetting then 0,0 added
							pos_offset_[rot] = pixel_distance(point(0, 1), min_coord, g_hex_tile_size);// - point(0, 72);;
						} else {
							// even just need to choose the minimum x/y tile. -- done above.
							pos_offset_[rot] = pixel_distance(center_, min_coord, g_hex_tile_size) + point(0, g_hex_tile_size);
						}
					}
				}
			}
		}

		if(!td->getPosition().empty()) {
			tile_data_.emplace_back(std::move(td));
		}
	}

	point TerrainRule::calcOffsetForRotation(int rot)
	{
		if(image_.empty()) {
			return point();
		}
		return pos_offset_[rot];
	}

	// return false to remove this rule. true if it should be kept.
	bool TerrainRule::tryEliminate()
	{
		// If rule has no images, we keep it since it may have flags.
		bool has_image = false;
		for(auto& td : tile_data_) {
			has_image |= td->hasImage();
		}
		if(!has_image && image_.empty()) {
			return true;
		}

		// Basically we should check these as part of initialisation and discard if the particular combination specified in the
		// image tag isn't valid.
		bool ret = false;
		for(auto& td : tile_data_) {
			ret |= td->eliminate(rotations_);
		}
		for(auto& img : image_) {
			const bool keep = img->eliminate(rotations_);
			ret |= keep;
			//if(!keep) {
			//	LOG_INFO("would eliminate: " << img->toString());
			//}
		}
		return ret;
	}

	void TerrainRule::applyImage(HexObject* hex, int rot)
	{
		point offs = calcOffsetForRotation(rot);
		for(const auto& img : image_) {
			if(img) {
				hex->addImage(img->genHolder(rot, offs));
			}
		}
	}

	TerrainRulePtr TerrainRule::create(const variant& v)
	{
		auto tr = std::make_shared<TerrainRule>(v);
		tr->preProcessMap(v["tile"]);
		return tr;
	}

	TileRule::TileRule(TerrainRulePtr parent, const variant& v)
		: parent_(parent),
		  position_(),
		  pos_(v["pos"].as_int32(0)),
		  type_(),
		  set_flag_(),
		  no_flag_(),
		  has_flag_(),
		  image_(nullptr),
		  pos_rotations_(),
		  min_pos_()
	{
		if(v.has_key("x") || v.has_key("y")) {
			position_.emplace_back(v["x"].as_int32(0), v["y"].as_int32(0));
		}
		std::vector<std::string> set_no_flag;
		if(v.has_key("set_no_flag")) {
			set_no_flag = v["set_no_flag"].as_list_string();
		}
		if(v.has_key("set_flag")) {
			set_flag_ = v["set_flag"].as_list_string();
		}
		for(const auto& snf : set_no_flag) {
			set_flag_.emplace_back(snf);
		}
		if(v.has_key("no_flag")) {
			no_flag_ = v["no_flag"].as_list_string();
		}
		for(const auto& snf : set_no_flag) {
			no_flag_.emplace_back(snf);
		}
		if(v.has_key("has_flag")) {
			has_flag_ = v["has_flag"].as_list_string();
		}
		if(v.has_key("type")) {
			type_ = v["type"].as_list_string();
		}
		// ignore name as I've not seen any instances in a tile def.
		if(v.has_key("image")) {
			image_.reset(new TileImage(v["image"]));
		}
	}

	// To match * type
	TileRule::TileRule(TerrainRulePtr parent)
		: parent_(parent),
		  position_(),
		  pos_(0),
		  type_(),
		  set_flag_(),
		  no_flag_(),
		  has_flag_(),
		  image_(nullptr),
		  pos_rotations_(),
		  min_pos_()
	{
		type_.emplace_back("*");
	}

	void TileRule::center(const point& from_center, const point& to_center)
	{
		for(auto& p : position_) {
	        int x_p, y_p, z_p;
	        int x_c, y_c, z_c;
            hex::evenq_to_cube_coords(p, &x_p, &y_p, &z_p);
            hex::evenq_to_cube_coords(from_center, &x_c, &y_c, &z_c);

            const int p_from_c_x = x_p - x_c;
            const int p_from_c_y = y_p - y_c;
            const int p_from_c_z = z_p - z_c;

			int x_r, y_r, z_r;
			hex::evenq_to_cube_coords(to_center, &x_r, &y_r, &z_r);
			p = hex::cube_to_evenq_coords(x_r + p_from_c_x, y_r + p_from_c_y, z_r + p_from_c_z);
		}
	}

	bool TileRule::eliminate(const std::vector<std::string>& rotations)
	{
		if(image_) {
			return image_->eliminate(rotations);
		}
		//return true;
		return false;
	}

	std::string TileRule::toString()
	{
		std::stringstream ss;
		ss << "TileRule: ";
		bool no_comma = true;
		if(!has_flag_.empty()) {
			ss << "has:";
			for(auto& hf : has_flag_) {
				ss << (no_comma ? "" : ",") << " \"" << hf << "\"";
				if(no_comma) {
					no_comma = false;
				}
			}
		}

		if(!set_flag_.empty()) {
			no_comma = true;
			ss << "; set:";
			for(auto& sf : set_flag_) {
				ss << (no_comma ? "" : ",") << " \"" << sf << "\"";
				if(no_comma) {
					no_comma = false;
				}
			}
		}

		if(!no_flag_.empty()) {
			no_comma = true;
			ss << "; no:";
			for(auto& nf : no_flag_) {
				ss << (no_comma ? "" : ",") << " \"" << nf << "\"";
				if(no_comma) {
					no_comma = false;
				}
			}
		}

		no_comma = true;
		ss << "; types:";
		for(auto& type : type_) {
			ss << (no_comma ? "" : ",") << " \"" << type << "\"";
			if(no_comma) {
				no_comma = false;
			}
		}

		no_comma = true;
		ss << "; positions:";
		for(auto& pos : position_) {
			ss << (no_comma ? "" : ", ") << pos;
			if(no_comma) {
				no_comma = false;
			}
		}

		if(image_) {
			ss << "; image: " << image_->toString();
		}
		return ss.str();
	}

	bool TileRule::matchFlags(const HexObject* obj, TerrainRule* tr, const std::vector<std::string>& rs, int rot)
	{
		const auto& has_flag = has_flag_.empty() ? tr->getHasFlags() : has_flag_;
		for(auto& f : has_flag) {
			if(!obj->hasFlag(rot_replace(f, rs, rot))) {
				return false;
			}
		}
		const auto& no_flag = no_flag_.empty() ? tr->getNoFlags() : no_flag_;
		for(auto& f : no_flag) {
			if(obj->hasFlag(rot_replace(f, rs, rot))) {
				return false;
			}
		}
		return true;
	}

	bool TileRule::match(const HexObject* obj, TerrainRule* tr, const std::vector<std::string>& rs, int rot)
	{
		if(obj == nullptr) {
			/*for(auto& type : type_) {
				if(type == "*") {
					return true;
				}
			}*/
			return false;
		}

		const std::string& hex_type_full = obj->getFullTypeString();
		const std::string& hex_type = obj->getTypeString();
		bool invert_match = false;
		bool tile_match = true;
		for(auto& type : type_) {
			type = rot_replace(type, rs, rot);
			if(type == "!") {
				invert_match = !invert_match;
				continue;
			}
			const bool matches = type == "*" || string_match(type, hex_type_full) || string_match(type, hex_type);
			if(!matches) {
				if(invert_match == true) {
					tile_match = true;
				} else {
					tile_match = false;
				}
			} else {
				if(invert_match == false) {
					tile_match = true;
				} else {
					tile_match = false;
				}
				break;
			}
		}

		if(tile_match) {
			if(!matchFlags(obj, tr, rs, rot)) {
				return false;
			}

			const auto& set_flag = set_flag_.empty() ? tr->getSetFlags() : set_flag_;
			for(auto& f : set_flag) {
				if(obj != nullptr) {
					obj->addTempFlag(rot_replace(f, rs, rot));
				}
			}
		}

		return tile_match;
	}

	void TileRule::applyImage(HexObject* hex, int rot)
	{
		if(image_) {
			hex->addImage(image_->genHolder(rot, point()));
		}
	}

	TileImage::TileImage(const variant& v)
		: layer_(v["layer"].as_int32(0)),
		  image_name_(v["name"].as_string_default("")),
		  random_start_(v["random_start"].as_bool(true)),
		  base_(),
		  center_(),
		  opacity_(1.0f),
		  crop_(),
		  variants_(),
		  variations_(),
		  image_files_(),
		  animation_frames_(),
		  animation_timing_(v["animation_timing"].as_int32(0)),
		  is_animated_(false)
	{
		if(v.has_key("O")) {
			opacity_ = v["O"]["param"].as_float();
		}
		if(v.has_key("CROP")) {
			crop_ = rect(v["CROP"]["param"]);
		}
		if(v.has_key("base")) {
			base_ = point(v["base"]);
		}
		if(v.has_key("center")) {
			center_ = point(v["center"]);
		}
		if(v.has_key("variant")) {
			for(const auto& ivar : v["variant"].as_list()) {
				variants_.emplace_back(ivar);
			}
		}
		if(v.has_key("animation_frames")) {
			animation_frames_ = v["animation_frames"].as_list_int();
			is_animated_ = true;
		}
		if(v.has_key("variations")) {
			auto vars = v["variations"].as_list_string();
			auto pos = image_name_.find("@V");
			auto pos_r = image_name_.find("@R");
			if(pos_r != std::string::npos) {
				variations_ = vars;
			} else {
				for(const auto& var : vars) {
					std::string name = image_name_.substr(0, pos) + var + image_name_.substr(pos + 2);
					if(terrain_info_exists(name)) {
						variations_.emplace_back(var);
					}
				}
			}
		}
	}

	const std::string& TileImage::getNameForRotation(int rot)
	{
		auto it = image_files_.find(rot);
		if(it == image_files_.end()) {
			static std::string res;
			return res;
		}
		ASSERT_LOG(it != image_files_.end(), "No image for rotation: " << rot << " : " << toString());
		ASSERT_LOG(!it->second.empty(), "No files for rotation: " << rot);
		return it->second[rng::generate() % it->second.size()];
	}

	bool TileImage::isValidForRotation(int rot)
	{
		return image_files_.find(rot) != image_files_.end();
	}

	std::string TileImage::toString() const
	{
		std::stringstream ss;
		ss << "name:" << image_name_ << "; layer(" << layer_ << "); base: " << base_;
		if(!variations_.empty()) {
			ss << "; variations:";
			for(auto& var : variations_) {
				ss << " " << var;
			}
		}
		return ss.str();
	}

	ImageHolder TileImage::genHolder(int rot, const point& offs)
	{
		ImageHolder res;
		res.name = getNameForRotation(rot);
		res.base = getBase();
		res.center = getCenter();
		res.crop = getCropRect();
		res.is_animated = is_animated_;
		res.layer = getLayer();
		res.offset = offs;
		res.opacity = getOpacity();
		if(is_animated_) {
			res.animation_frames = image_files_[rot];
		}
		res.animation_timing = animation_timing_;
		return res;
	}

	bool TileImage::eliminate(const std::vector<std::string>& rotations)
	{
		// Calculate whether particular rotations are valid.
		// return true if we should keep this, false if there are
		// no valid terrain images available.
		auto pos_v = image_name_.find("@V");
		auto pos_r = image_name_.find("@R");
		auto pos_a = image_name_.find("@A");

		if(pos_a != std::string::npos) {
			ASSERT_LOG(pos_v == std::string::npos && pos_r == std::string::npos,
				"Found an animation string with @V or @R specifier which isn't valid. " << image_name_);
			if(!animation_frames_.empty()) {
				for(const auto& ani : animation_frames_) {
					std::stringstream ss;
					ss 	<< image_name_.substr(0, pos_a)
						<< std::dec << std::setw(2) << std::setfill('0') << ani
						<< image_name_.substr(pos_a + 2);
					const std::string img_name = ss.str();
					if(terrain_info_exists(img_name)) {
						image_files_[0].emplace_back(img_name);
					}
				}
			} else {
				if(terrain_info_exists(image_name_)) {
					image_files_[0].emplace_back(image_name_);
				}
			}
			return !image_files_.empty();
		}

		if(pos_r == std::string::npos) {
			if(!variations_.empty()) {
				for(const auto& var : variations_) {
					pos_v = image_name_.find("@V");
					std::string img_name = image_name_.substr(0, pos_v) + var + image_name_.substr(pos_v + 2);
					if(terrain_info_exists(img_name)) {
						image_files_[0].emplace_back(img_name);
					}
				}
			} else {
				if(terrain_info_exists(image_name_)) {
					image_files_[0].emplace_back(image_name_);
				}
			}
			return !image_files_.empty();
		}
		// rotate all the combinations and test them
		for(int rot = 0; rot != 6; ++rot) {
			std::string name = rot_replace(image_name_, rotations, rot);
			if(pos_v == std::string::npos) {
				if(terrain_info_exists(name)) {
					image_files_[rot].emplace_back(name);
				}
				continue;
			}
			for(const auto& var : variations_) {
				pos_v = name.find("@V");
				std::string img_name = name.substr(0, pos_v) + var + name.substr(pos_v + 2);
				if(terrain_info_exists(img_name)) {
					image_files_[rot].emplace_back(img_name);
				} else {
				}
			}
		}

		return !image_files_.empty();
	}

	TileImageVariant::TileImageVariant(const variant& v)
		: tod_(v["tod"].as_string_default("")),
		  name_(v["name"].as_string_default("")),
		  random_start_(v["random_start"].as_bool(true)),
		  has_flag_(),
		  crop_(),
		  animation_frames_(),
		  animation_timing_(v["animation_timing"].as_int32(0)),
		  layer_(v["layer"].as_int32(0))
	{
		if(v.has_key("has_flag")) {
			has_flag_ = v["has_flag"].as_list_string();
		}
		if(v.has_key("CROP")) {
			crop_ = rect(v["CROP"]["param"]);
		}
		if(v.has_key("animation_frames")) {
			animation_frames_ = v["animation_frames"].as_list_int();
		}
	}

	std::string TileImage::getName() const
	{
		// XXX WIP
		std::string name = image_name_;
		auto pos = name.find("@V");
		if(!variations_.empty() && pos != std::string::npos) {
			int index = rng::generate() % variations_.size();
			const std::string& var = variations_[index];
			name = name.substr(0, pos) + var + name.substr(pos + 2);
		}
		return name;
	}

	bool TerrainRule::match(HexObject* hex)
	{
		const int max_loop = rotations_.empty() ? 1 : rotations_.size();

		for(int rot = 0; rot != max_loop; ++rot) {
			if(mod_position_) {
				auto& pos = hex->getPosition();
				if((pos.x % mod_position_->x) != 0 || (pos.y % mod_position_->y) != 0) {
					continue;
				}
			}

			std::vector<std::pair<HexObject*, TileRule*>> obj_to_set_flags;
			// We expect tiles to have position data.
			bool tile_match = true;

			if(!image_.empty()) {
				bool res = false;
				for(const auto& img : image_) {
					res |= img->isValidForRotation(rot);
				}
				if(!res) {
					// XXX should we check for images in tile tags?
					continue;
				}
			}

			bool match_pos = true;
			auto td_it = tile_data_.cbegin();
			for(; td_it != tile_data_.cend() && match_pos; ++td_it) {
				const auto& td = *td_it;
				ASSERT_LOG(td->hasPosition(), "tile data doesn't have an x,y position.");
				const auto& pos_data = td->getPosition();

				for(const auto& p : pos_data) {
					//point rot_p = sub_hex_coord(add_hex_coord(hex.getPosition(), rotate_point(rot, center_, p)), center_);
					point rot_p = rotate_point(rot, add_hex_coord(center_, hex->getPosition()), add_hex_coord(p, hex->getPosition()));
					auto new_obj = const_cast<HexObject*>(hex->getTileAt(rot_p));
					if(td->match(new_obj, this, rotations_, rot)) {
						//match_pos = true;
						if(new_obj) {
							obj_to_set_flags.emplace_back(std::make_pair(new_obj, td.get()));
						}
					} else {
						match_pos = false;
						if(new_obj) {
							new_obj->clearTempFlags();
						}
						break;
					}
				}
			}
			if(!match_pos) {
				tile_match = false;
				for(auto& obj : obj_to_set_flags) {
					obj.first->clearTempFlags();
				}
				obj_to_set_flags.clear();
			}

			if(tile_match) {
				if(probability_ != 100) {
					auto rand_no = rng::generate() % 100;
					if(rand_no > probability_) {
						for(auto& obj : obj_to_set_flags) {
							obj.first->clearTempFlags();
						}
						obj_to_set_flags.clear();
						continue;
					}
				}
				// XXX need to fix issues when other tiles have images that need to match a different hex
				//tile_data_.front()->applyImage(&hex, rotations_, rot);
				applyImage(hex, rot);
				for(auto& obj : obj_to_set_flags) {
					obj.first->setTempFlags();
					obj.second->applyImage(obj.first, rot);
				}
			}
		}
		return false;
	}

	bool TerrainRule::match(const HexMapPtr& hmap)
	{
		if(absolute_position_) {
			ASSERT_LOG(tile_data_.size() != 1, "Number of tiles is not correct in rule.");
			if(!tile_data_[0]->match(hmap->getTileAt(*absolute_position_), this, std::vector<std::string>(), 0)) {
				return false;
			}
		}

		// check rotations.
		ASSERT_LOG(rotations_.size() == 6 || rotations_.empty(), "Set of rotations not of size 6(" << rotations_.size() << ").");

		for(auto& hex : hmap->getTilesMutable()) {
			match(&hex);
		}
		return false;
	}
}

UNIT_TEST(point_rotate)
{
	CHECK_EQ(rotate_point(0, point(2, 2), point(2, 1)), point(2, 1));
	CHECK_EQ(rotate_point(1, point(2, 2), point(2, 1)), point(3, 2));
	CHECK_EQ(rotate_point(2, point(2, 2), point(2, 1)), point(3, 3));
	CHECK_EQ(rotate_point(3, point(2, 2), point(2, 1)), point(2, 3));
	CHECK_EQ(rotate_point(4, point(2, 2), point(2, 1)), point(1, 3));
	CHECK_EQ(rotate_point(5, point(2, 2), point(2, 1)), point(1, 2));
	CHECK_EQ(rotate_point(1, point(3, 3), point(3, 2)), point(4, 2));
}

UNIT_TEST(string_match)
{
	CHECK_EQ(string_match("*", "Any string"), true);
	CHECK_EQ(string_match("Chs", "Ch"), false);
	CHECK_EQ(string_match("G*", "Gg"), true);
	CHECK_EQ(string_match("G*^Fp", "Gg^Fp"), true);
	CHECK_EQ(string_match("Re", "Rd"), false);
	CHECK_EQ(string_match("*^Bsb|", "Gg^Bsb|"), true);
	CHECK_EQ(string_match("*^Bsb|", "Gg^Fp"), false);
	//CHECK_EQ(string_match("Aa", "Aa^Fpa"), true);
}

UNIT_TEST(rot_replace)
{
	CHECK_EQ(rot_replace("transition-@R0-@R1-x", std::vector<std::string>{"n", "ne", "se", "s", "sw", "nw"}, 1), "transition-ne-se-x");
	CHECK_EQ(rot_replace("xyzzy", std::vector<std::string>{}, 0), "xyzzy");
	CHECK_EQ(rot_replace("transition-@R0", std::vector<std::string>{"n", "ne", "se", "s", "sw", "nw"}, 0), "transition-n");
	CHECK_EQ(rot_replace("transition-@R0", std::vector<std::string>{"n", "ne", "se", "s", "sw", "nw"}, 1), "transition-ne");
	CHECK_EQ(rot_replace("transition-@R0", std::vector<std::string>{"n", "ne", "se", "s", "sw", "nw"}, 5), "transition-nw");
}

UNIT_TEST(pixel_distance)
{
	CHECK_EQ(pixel_distance(point(0,0), point(0,1), 72), point(0, 72));
	CHECK_EQ(pixel_distance(point(0,0), point(1,0), 72), point(54, -36));
}