/** \file
 *
 *  Contains depth VBS mission tests.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include <cxxtest/TestSuite.h>

#include <chrono>

#include <lrauv_system_tests/TestFixture.hh>

#include "LRAUVController.h"
#include "TestConstants.h"

using namespace lrauv_system_tests;
using namespace std::literals::chrono_literals;

class DepthVBSMissionTestSuite : public CxxTest::TestSuite
{
public:
    void testMission()
    {
        TestFixtureWithVehicle fixture(
            GZ_WORLDS_PATH "/buoyant_tethys.sdf", "tethys" );
        auto &observer = fixture.VehicleObserver();
        // Limit window sizes ensuring overlap between loop iterations
        observer.LimitTo( 3min + 10s );

        // Check initial pose (facing North)
        fixture.Step();
        const auto &times = observer.Times();
        const auto &poses = observer.Poses();
        const auto &linearVelocities = observer.LinearVelocities();
        TS_ASSERT_DELTA( 0.0, poses.back().Pos().X(), 1e-6 );
        TS_ASSERT_DELTA( 0.0, poses.back().Pos().Y(), 1e-6 );
        TS_ASSERT_DELTA( -0.5, poses.back().Pos().Z(), 1e-6 );
        TS_ASSERT_DELTA( 0.0, poses.back().Rot().Roll(), 1e-6 );
        TS_ASSERT_DELTA( 0.0, poses.back().Rot().Pitch(), 1e-6 );
        TS_ASSERT_DELTA( 0.0, poses.back().Rot().Yaw(), 1e-6 );

        // Launch mission
        auto controller = LRAUVController::Execute(
        {
            "run RegressionTests/GazeboTests/testDepthVBS.xml quitAtEnd"
        } );
        auto maxLinearVelocity = gz::math::Vector3d::Zero;
        // Run until mission timeout (30min).
        for( size_t _ = 0; _ < 10; ++_ )
        {
            TS_ASSERT_LESS_THAN( 0u, fixture.Step( 3min ) );
            for( size_t i = 0; i < times.size(); ++i )
            {
                // Vehicle roll (about +Y since vehicle is facing North) should be constant
                TS_ASSERT_DELTA( poses[i].Rot().Euler().Y(), 0, 1e-2 );

                if( times[i] > 3min ) // Initialization is complete
                {
                    // Vehicle should not go vertical when tilting nose
                    TS_ASSERT_LESS_THAN( poses[i].Rot().Euler().X(), GZ_DTOR( 10 ) );
                    TS_ASSERT_LESS_THAN( GZ_DTOR( -10 ), poses[i].Rot().Euler().X() );
                }

                // Vehicle should not exceed 20m depth
                // NOTE: Although the mission has a target depth of 10m, the vehicle has
                // a tendency to overshoot first. Eventually the vehicle will reach steady
                // state, however at this point we are just checking for excess overshoot.
                TS_ASSERT_LESS_THAN( -20.0, poses[i].Pos().Z() );

                if( linearVelocities[i].Length() > maxLinearVelocity.Length() )
                {
                    maxLinearVelocity = linearVelocities[i];
                }
            }
        }
        TS_ASSERT( controller.Kill( SIGINT ) );
        TS_ASSERT_EQUALS( SIGINT, controller.Wait() );

        // Vehicle's final depth should be near the 10m mark
        TS_ASSERT_DELTA( poses.back().Pos().Z(), -10.0, 1e-1 );

        // // Vehicle should have near zero Z velocity at the end.
        TS_ASSERT_DELTA( linearVelocities.back().Z(), 0, 1e-1 );

        // Expect velocity to approach zero. At the end of the test, the vehicle may
        // not have actually reached zero as it may still be translating or yawing,
        // but its velocity should be less than the maximum velocity.
        TS_ASSERT_LESS_THAN( linearVelocities.back().Length(), maxLinearVelocity.Length() );
    }
};