/** \file
 *
 *  Contains unit tests for the OffshoreEnvelope class
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef _OFFSHOREENVELOPE_TEST_H_
#define _OFFSHOREENVELOPE_TEST_H_

#include "OffshoreEnvelope.h"
#include "OffshoreEnvelopeIF.h"

#include "component/BehaviorRegistry.h"
#include "component/TestAsyncComponent.h"
#include "controlModule/SpeedControlIF.h"
#include "navigationModule/NavChart.h"
#include "data/Slate.h"
#include "data/UniversalDataWriter.h"
#include "io/FileOutStream.h"
#include "io/NavChartDb.h"
#include "logger/LogEngine.h"
#include "logger/LoggerRules.h"
#include "logger/TextLogWriter.h"
#include "missionScript/MissionItem.h"
#include "missionScript/MissionNode.h"
#include "module/Config.h"
#include "units/Units.h"

#include <cxxtest/TestSuite.h>

/**
 *  Units tests for OffshoreEnvelope class
 *
 *  \ingroup modules_guidance
 */
class OffshoreEnvelope_Test : public CxxTest::TestSuite
{
public:

    LogEngine syslogEngine_;

    // Ensures the syslog is empty at the end of the run.
    virtual void tearDown( void )
    {

        FileOutStream stdOutStream( stdout );
        syslogEngine_.addAssociation( new TextLogWriter( stdOutStream ),
                                      new TypeRule( LogEntry::SYSLOG_LOG_ENTRY ) );
        syslogEngine_.processQueue( Timespan::ZERO_TIMESPAN );
    }

    // run test
    // We start with the vehicle close to shore, heading towards shore.
    // We expect the behavior to turn the vehicle around.
    // We run 100 cycles, or about 40 seconds.
    // At the end, we expect the vehicle to be further wes than where it started.
    virtual void testRun( void )
    {
        // Make sure NavChart is set up...
        NavChartDb navChartDb;
        // Dump the critical errors from setting up navchartDb with no real config variables
        syslogEngine_.processQueue( Timespan::ZERO_TIMESPAN );
        // Fake the chartsCfg
        Str chartsCfg( "US1WC07M,US2WC11M,US3CA52M,US4CA60M,US5CA50M,US5CA61M,US5CA62M,US5CA83M" );
        navChartDb.setChartsCfg( chartsCfg );
        navChartDb.initialize();
        int maxLoops = 1000000;
        while( !navChartDb.ready() && maxLoops > 0 )
        {
            navChartDb.execute();
            --maxLoops;
        }

        TS_ASSERT_LESS_THAN( 0, maxLoops );

        // Fake a mission...
        BehaviorRegistry::Initialize();
        BehaviorRegistry::Insert( OffshoreEnvelopeIF::NAME, &OffshoreEnvelope::CreateBehavior, NULL );
        TestAsyncComponent testComponent( "OffshoreEnvelope_TestComponent" );

        // Build up the behavior as if it were expressed in XML
        MissionNode offshoreEnvelopeNode( "Guidance:OffshoreEnvelope" );
        offshoreEnvelopeNode.setAttribute( "RunIn", "parallel" );
        MissionNode* minOffshoreSettingNode( offshoreEnvelopeNode.appendChild( "Setting" ) );
        minOffshoreSettingNode->appendChild( "Guidance:OffshoreEnvelope.minOffshore" );
        minOffshoreSettingNode->appendChild( "Units:meter" );
        minOffshoreSettingNode->appendChild( "Value" )->appendChildText( "1000" ); // 1 km
        MissionNode* maxOffshoreSettingNode( offshoreEnvelopeNode.appendChild( "Setting" ) );
        maxOffshoreSettingNode->appendChild( "Guidance:OffshoreEnvelope.maxOffshore" );
        maxOffshoreSettingNode->appendChild( "Units:meter" );
        maxOffshoreSettingNode->appendChild( "Value" )->appendChildText( "100000" ); // 100 km

        // Convert the virtual XML to a behavior
        MissionItem* offshoreEnvelopeMissionItem( MissionItem::Instance( NULL, &offshoreEnvelopeNode, 0, testComponent.getLogger() ) );
        OffshoreEnvelope* offshoreEnvelopePtr = ( OffshoreEnvelope* )( offshoreEnvelopeMissionItem->getComponent() );
        OffshoreEnvelope& offshoreEnvelope( *offshoreEnvelopePtr );

        // Initialize the behavior
        offshoreEnvelope.initialize();

        /// Slate variables on their way to DynamicControl
        DataReader* readerControlHeadingMode = testComponent.newDataReader( HorizontalControlIF::HORIZONTAL_MODE );
        DataReader* readerControlHeading = testComponent.newDataReader( HorizontalControlIF::HEADING_CMD );
        DataReader* readerControlSpeed = testComponent.newDataReader( SpeedControlIF::SPEED_CMD );

        // Universals
        UniversalDataWriter* writerLatitudeFix = testComponent.newUniversalWriter( UniversalURI::LATITUDE_FIX, Units::DEGREE, 1.0 );
        UniversalDataWriter* writerLatitude = testComponent.newUniversalWriter( UniversalURI::LATITUDE, Units::DEGREE, 1.0 );
        UniversalDataWriter* writerLongitude = testComponent.newUniversalWriter( UniversalURI::LONGITUDE, Units::DEGREE, 1.0 );
        UniversalDataWriter* writerOrientation = testComponent.newUniversalWriter( UniversalURI::PLATFORM_ORIENTATION, Units::RADIAN, 1.0 );

        // Universals...
        // This should send the vehicle offshore
        double latitude = D2R( 36.8 );
        double longitude = D2R( -121.8 );
        float orientation = M_PI_2;

        // Interrogate nav chart
        writerLatitude->write( Units::RADIAN, latitude );
        writerLongitude->write( Units::RADIAN, longitude );
        NavChart navChart( NULL );
        navChart.initialize();
        writerLatitudeFix->write( Units::RADIAN, latitude );
        for( int i = 0; i < 1000; ++i )
        {
            navChart.execute();
            navChartDb.execute();
        }

        // Let's run a loop
        float period = 0.4;
        float yaw_step = M_PI / 36 * period;
        int horizontalMode( HorizontalControlIF::NONE );
        float speed( 0.0 );
        float heading( M_PI_2 );

        for( int i = 0; i < 100; ++i )
        {
            writerLatitude->write( Units::RADIAN, latitude );
            writerLongitude->write( Units::RADIAN, longitude );
            writerOrientation->write( Units::RADIAN, orientation );

            navChart.execute();
            offshoreEnvelope.execute();

            bool readOk = readerControlHeadingMode->read( Units::ENUM, horizontalMode );
            TS_ASSERT( readOk );
            TS_ASSERT_EQUALS( horizontalMode, ( int )HorizontalControlIF::HEADING );

            readOk = readerControlSpeed->read( Units::METER_PER_SECOND, speed );
            TS_ASSERT( readOk );
            TS_ASSERT_LESS_THAN( 0.0, speed );

            readOk = readerControlHeading->read( Units::RADIAN, heading );
            TS_ASSERT( readOk );
            TS_ASSERT_DIFFERS( heading, M_PI_2 );

            if( heading != orientation )
            {
                float diff = fabs( heading - orientation );
                if( diff >  yaw_step )
                {
                    float step = diff / ( heading - orientation ) * yaw_step;
                    orientation += step;
                }
                else
                {
                    orientation = heading;
                }
            }
            Location::AtBearing( orientation, speed * period, latitude, longitude );
        }

        // cleanup
        offshoreEnvelope.uninitialize();
        delete offshoreEnvelopeMissionItem;
        BehaviorRegistry::Uninitialize();
        // the test!
        TS_ASSERT_LESS_THAN( longitude, D2R( -121.80 ) );

        navChart.uninitialize();
        navChartDb.uninitialize();
    }

};

#endif // _OFFSHOREENVELOPE_TEST_H