/*
 * Copyright (C) 2021 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <gtest/gtest.h>

#include "QuaternionUtil.h"
#include "TestUtil.h"

using Eigen::Quaternionf;
using Eigen::Vector3f;

namespace android {
namespace media {
namespace {

TEST(QuaternionUtil, RotationVectorToQuaternion) {
    // 90 degrees around Z.
    Vector3f rot = {0, 0, M_PI_2};
    Quaternionf quat = rotationVectorToQuaternion(rot);
    ASSERT_EQ(quat * Vector3f(1, 0, 0), Vector3f(0, 1, 0));
    ASSERT_EQ(quat * Vector3f(0, 1, 0), Vector3f(-1, 0, 0));
    ASSERT_EQ(quat * Vector3f(0, 0, 1), Vector3f(0, 0, 1));
}

TEST(QuaternionUtil, QuaternionToRotationVector) {
    Quaternionf quat = Quaternionf::FromTwoVectors(Vector3f(1, 0, 0), Vector3f(0, 1, 0));
    Vector3f rot = quaternionToRotationVector(quat);
    ASSERT_EQ(rot, Vector3f(0, 0, M_PI_2));
}

TEST(QuaternionUtil, RoundTripFromQuaternion) {
    Quaternionf quaternion = Quaternionf::UnitRandom();
    EXPECT_EQ(quaternion, rotationVectorToQuaternion(quaternionToRotationVector(quaternion)));
}

TEST(QuaternionUtil, RoundTripFromVector) {
    Vector3f vec{0.1, 0.2, 0.3};
    EXPECT_EQ(vec, quaternionToRotationVector(rotationVectorToQuaternion(vec)));
}

}  // namespace
}  // namespace media
}  // namespace android