/** \file
 *
 *  Contains pitch mass 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 PitchMassMissionTestSuite : public CxxTest::TestSuite
{
public:
    void testMission()
    {
        TestFixtureWithVehicle fixture(
            GZ_WORLDS_PATH "/buoyant_tethys_at_depth.sdf", "tethys" );
        auto &observer = fixture.VehicleObserver();
        // Limit window sizes ensuring overlap between loop iterations
        observer.LimitTo( 2min + 10s );

        // Check initial X
        fixture.Step();
        const auto &times = observer.Times();
        const auto &poses = observer.Poses();
        TS_ASSERT_DELTA( 0.0, poses.back().Pos().X(), 1e-6 );

        // Launch mission
        //   SetSpeed.speed = 0 m/s^2
        //   Point.rudderAngle = 0 deg
        //   Buoyancy.position = neutral
        //   Pitch.pitch = 20 deg
        //   Pitch.elevatorAngle = 0
        auto controller = LRAUVController::Execute(
        {
            // Force neutral buoyancy to prevent the vehicle
            // from surfacing during controller startup
            "configSet VerticalControl.buoyancyDefault 500 cc",
            "run RegressionTests/GazeboTests/testPitchMass.xml quitAtEnd"
        } );

        double totalPitchChange = 0, prevPitch = 0;
        bool firstPitch = false, reachedTarget = false;
        for( size_t _ = 0; _ < 5; ++_ )
        {
            TS_ASSERT_LESS_THAN( 0u, fixture.Step( 2min ) );

            // Vehicle should have a max pitch of 20 degrees
            // Since the vehicle is facing North (+Y):
            // Euler.Y = roll
            // Euler.X = pitch
            // Euler.Z = yaw
            for( size_t i = 0; i < times.size(); ++i )
            {
                // Check position holds
                TS_ASSERT_DELTA( poses[i].Pos().X(), 0.0, 2e-1 );
                TS_ASSERT_DELTA( poses[i].Pos().Y(), 0.0, 2e-1 );
                TS_ASSERT_DELTA( poses[i].Pos().Z(), -10.0, 2e-1 );

                if( times[i] > 3min ) // Initialization is complete
                {
                    // Max Pitch 20 degrees, allowing for an error of
                    // up to 5 degrees to accomodate for oscillations
                    // during pitch control.
                    TS_ASSERT_LESS_THAN( poses[i].Rot().Euler().X(), GZ_DTOR( 25 ) );
                    TS_ASSERT_LESS_THAN( GZ_DTOR( -5 ), poses[i].Rot().Euler().X() );
                }

                // No roll or yaw
                TS_ASSERT_DELTA( poses[i].Rot().Euler().Y(), GZ_DTOR( 0 ), 1e-2 );
                TS_ASSERT_DELTA( poses[i].Rot().Euler().Z(), GZ_DTOR( 0 ), 1e-2 );

                // Used later for oscillation check.
                if( !firstPitch )
                {
                    totalPitchChange += std::abs( poses[i].Rot().Euler().X() - prevPitch );
                    firstPitch = true;
                }

                // Check if we cross the 20 degree mark
                if( prevPitch <= GZ_DTOR( 20 ) && poses[i].Rot().Euler().X() >= GZ_DTOR( 20 ) )
                {
                    reachedTarget = true;
                }

                prevPitch = poses[i].Rot().Euler().X();
            }
        }
        TS_ASSERT( controller.Kill( SIGINT ) );
        TS_ASSERT_EQUALS( SIGINT, controller.Wait() );

        // Check for oscillation by summing over abs delta in pitch
        // Essentially \Sigma abs(f'(x)) < C. In this case C should be near 2*20
        // degrees as the vehicle first pitches down and then comes back up.
        TS_ASSERT_LESS_THAN_EQUALS( totalPitchChange, GZ_DTOR( 20 ) * 2. );

        // Make sure the vehicle actually pitched to 20 degrees.
        TS_ASSERT( reachedTarget );
    }
};