/** \file
 *
 *  Contains pitch and depth mass 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 PitchAndDepthMassVBSMissionTestSuite : 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( 2min + 10s );

        // Check initial pose (facing North)
        fixture.Step();
        const auto &times = observer.Times();
        const auto &poses = observer.Poses();
        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
        //   SetSpeed.speed = 0 m/s^2
        //   Point.rudderAngle = 0 deg
        //   Pitch.depth = 10 m
        //   Pitch.elevatorAngle = 0
        auto controller = LRAUVController::Execute(
        {
            "run RegressionTests/GazeboTests/testPitchAndDepthMassVBS.xml quitAtEnd"
        } );

        bool targetReached = false, firstSample = true;
        double prevZ = 0, totalDepthChange = 0;
        for( size_t _ = 0; _ < 5; ++_ )
        {
            TS_ASSERT_LESS_THAN( 0u, fixture.Step( 2min ) );
            for( size_t i = 0; i < times.size(); ++i )
            {
                // Vehicle should not breach the surface.
                TS_ASSERT_LESS_THAN_EQUALS( poses[i].Pos().Z(), 0.0 );
                // NOTE: vehicle target depth is 10m, but vertical
                // control exhibits considerable overshoot.
                TS_ASSERT_LESS_THAN( -20., poses[i].Pos().Z() );

                // Vehicle should exhibit minimal lateral translation.
                // FIXME: Hydrodynamic damping and graded buoyancy introduce
                // spurious thrusts. Reduce tolerances when fixed.
                TS_ASSERT_DELTA( poses[i].Pos().X(), 0, 0.3 );
                TS_ASSERT_DELTA( poses[i].Pos().Y(), 0, 5.0 );

                if( times[i] > 3min ) // Initialization is complete
                {
                    // Vehicle should hold a fixed angle about X
                    TS_ASSERT_DELTA( poses[i].Rot().Euler().X(), 0.0, 1e-1 );
                    TS_ASSERT_DELTA( poses[i].Rot().Euler().Y(), 0.0, 1e-2 );
                    TS_ASSERT_DELTA( poses[i].Rot().Euler().Z(), 0.0, 1e-2 );

                    if( !firstSample )
                    {
                        // Check we actually crossed the 10m mark
                        if( prevZ >= -10.0 && poses[i].Pos().Z() <= -10.0 )
                        {
                            targetReached = true;
                        }
                        // Use total depth change as a proxy for oscillations
                        totalDepthChange += std::fabs( poses[i].Pos().Z() - prevZ );
                    }
                    prevZ = poses[i].Pos().Z();
                    firstSample = false;
                }
            }
        }
        TS_ASSERT( controller.Kill( SIGINT ) );
        TS_ASSERT_EQUALS( SIGINT, controller.Wait() );

        // Vehicle should have reached the desired target.
        TS_ASSERT( targetReached );

        // Vehicle oscillations should be limited.
        TS_ASSERT_LESS_THAN( totalDepthChange, 30.0 );
    }
};