/** \file
 *
 *  Contains yo-yo circle 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 YoYoCircleMissionTestSuite : 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 );

        fixture.Step();
        const auto &poses = observer.Poses();
        const auto &times = observer.Times();
        const auto &linearVelocities = observer.LinearVelocities();
        const auto &angularVelocities = observer.AngularVelocities();
        TS_ASSERT_DELTA( 0.0, poses.back().Pos().X(), 1e-6 );

        // Launch mission
        //   Mass.position: default
        //   Buouancy.position: neutral
        //   Point.rudderAngle: 9 deg
        //   SetSpeed.speed: 1 m/s
        //   DepthEnvelope.minDepth: 2 m
        //   DepthEnvelope.maxDepth: 20 m
        //   DepthEnvelope.downPitch: -20 deg
        //   DepthEnvelope.upPitch: 20 deg
        auto controller = LRAUVController::Execute(
        {
            "run RegressionTests/GazeboTests/testYoYoCircle.xml quitAtEnd"
        } );

        for( size_t _ = 0; _ < 5; ++_ )
        {
            TS_ASSERT_LESS_THAN( 0, fixture.Step( 2min ) );
            for( size_t i = 0; i < times.size(); ++i )
            {
                // Pitch should be between -20 and 20 degrees
                TS_ASSERT_LESS_THAN( GZ_DTOR( -20 ), poses[i].Rot().Pitch() );
                TS_ASSERT_LESS_THAN( poses[i].Rot().Pitch(), GZ_DTOR( 20 ) );

                if( times[i] > 2min )
                {
                    // Check that the vehicle is actually moving
                    TS_ASSERT_LESS_THAN( 0.0, linearVelocities[i].Length() );
                }

                // Depth should be less than 20m, with some tolerance
                // to accommodate vertical control overshoot
                TS_ASSERT_LESS_THAN( -20.0 - 4.0, poses[i].Pos().Z() );
                if( times[i] > 3min )
                {
                    // Depth should be at greater than 2m after initial descent,
                    // with some tolerance to accommodate vertical control overshoot
                    TS_ASSERT_LESS_THAN( poses[i].Pos().Z(), -2.0 + 2.0 );
                }

                if( times[i] > 5min )
                {
                    // Once the vehicle achieves its full velocity the vehicle should have
                    // a nominal yaw rate of around 0.037-0.038rad/s. This means that the
                    // vehicle should keep spinning in a circle.
                    TS_ASSERT_DELTA( angularVelocities[i].Z(), 0.037, 0.0023 );

                    // At the same time the roll rate should be near zero
                    TS_ASSERT_DELTA( angularVelocities[i].Y(), 0, 1e-1 );

                    // And the linear velocity should be near 1m/s
                    TS_ASSERT_DELTA( linearVelocities[i].Length(), 1.0, 2e-1 );
                }
            }
        }
        TS_ASSERT( controller.Kill( SIGINT ) );
        TS_ASSERT_EQUALS( SIGINT, controller.Wait() );
    }
};