/** \file
 *
 *  Contains the FrontTracking class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "FrontTracking.h"
#include "FrontTrackingIF.h"
#include "estimationModule/StratificationFrontDetectorIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "WaitIF.h"
#include "data/Location.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

FrontTracking::FrontTracking( const Str& prefix, const Module* module )
    : Behavior( prefix + FrontTrackingIF::NAME, module, true, true ),
      northHdgSetting_( nan( "" ) ),
      southHdgSetting_( nan( "" ) ),
      eastHdgSetting_( nan( "" ) ),
      westHdgSetting_( nan( "" ) ),
      nEHdgSetting_( nan( "" ) ),
      nWHdgSetting_( nan( "" ) ),
      sEHdgSetting_( nan( "" ) ),
      sWHdgSetting_( nan( "" ) ),
      searchHdgSetting_( nan( "" ) ),
      searchHdgSettingPrevious_( nan( "" ) ),
      latNorthWallSetting_( nan( "" ) ),
      latSouthWallSetting_( nan( "" ) ),
      lonWestWallSetting_( nan( "" ) ),
      lonEastWallSetting_( nan( "" ) ),
      waitSecondsSetting_( nan( "" ) ),
      pastFrontNewSearchHdg_( nan( "" ) ),
      latitude_( nan( "" ) ),
      longitude_( nan( "" ) ),
      shapeZSetting_( false ),
      shapeNSetting_( false ),
      flagNorthSouthSweepSetting_( true ),
      flagNorthwardSweepSetting_( false ),
      flagEastwardSweepSetting_( false ),
      initialized_( false ),
      frontDetected_( false ),
      hitWallEnable_( true ),
      pastFrontNewHdgEnable_( false ),
      hitWallURI_( false ),
      hitEastWall_( false ),
      hitWestWall_( false ),
      hitNorthWall_( false ),
      hitSouthWall_( false ),
      validFrontDetectedURI_( false ),
      cntFrontDetection_( 0 ),
      cntIgnoreDetection_( 0 ),
      dataTimestamp_( Timestamp::NOT_SET_TIME )
{

    logger_.syslog( "Construct FrontTracking." );

    // Slate input setting variables
    northHdgSettingReader_ = newSettingReader( FrontTrackingIF::NORTHHDG_SETTING );
    southHdgSettingReader_ = newSettingReader( FrontTrackingIF::SOUTHHDG_SETTING );
    eastHdgSettingReader_ = newSettingReader( FrontTrackingIF::EASTHDG_SETTING );
    westHdgSettingReader_ = newSettingReader( FrontTrackingIF::WESTHDG_SETTING );
    nEHdgSettingReader_ = newSettingReader( FrontTrackingIF::NEHDG_SETTING );
    nWHdgSettingReader_ = newSettingReader( FrontTrackingIF::NWHDG_SETTING );
    sEHdgSettingReader_ = newSettingReader( FrontTrackingIF::SEHDG_SETTING );
    sWHdgSettingReader_ = newSettingReader( FrontTrackingIF::SWHDG_SETTING );
    searchHdgSettingReader_ = newSettingReader( FrontTrackingIF::SEARCHHDG_SETTING );
    latNorthWallSettingReader_ = newSettingReader( FrontTrackingIF::LATNORTHWALL_SETTING );
    latSouthWallSettingReader_ = newSettingReader( FrontTrackingIF::LATSOUTHWALL_SETTING );
    lonWestWallSettingReader_ = newSettingReader( FrontTrackingIF::LONWESTWALL_SETTING );
    lonEastWallSettingReader_ = newSettingReader( FrontTrackingIF::LONEASTWALL_SETTING );
    shapeZSettingReader_ = newSettingReader( FrontTrackingIF::SHAPEZ_SETTING );
    shapeNSettingReader_ = newSettingReader( FrontTrackingIF::SHAPEN_SETTING );
    flagNorthSouthSweepSettingReader_ = newSettingReader( FrontTrackingIF::FLAGNORTHSOUTHSWEEP_SETTING );
    flagNorthwardSweepSettingReader_ = newSettingReader( FrontTrackingIF::FLAGNORTHWARDSWEEP_SETTING );
    flagEastwardSweepSettingReader_ = newSettingReader( FrontTrackingIF::FLAGEASTWARDSWEEP_SETTING );
    waitSecondsSettingReader_ = newSettingReader( FrontTrackingIF::WAITSECONDS_SETTING );

    // Slate input measurements
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    headingReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION );

    horizontalModeReader_ = newDataReader( HorizontalControlIF::HORIZONTAL_MODE );
    frontReader_ = newDataReader( StratificationFrontDetectorIF::FRONT );

    // Slate output variables
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    headingCmdWriter_ = newDataWriter( HorizontalControlIF::HEADING_CMD );

    hitWallWriter_ = newDataWriter( FrontTrackingIF::HIT_WALL );
    validFrontDetectedWriter_ = newDataWriter( FrontTrackingIF::VALID_FRONT_DETECTED );

    // Argument output variables - must be defined at initialization
    frontDetectionWriter_ = NULL;
}

FrontTracking::~FrontTracking()
{}

void FrontTracking::readSettings( void )
{
    northHdgSettingReader_->read( Units::DEGREE, northHdgSetting_ );
    southHdgSettingReader_->read( Units::DEGREE, southHdgSetting_ );
    eastHdgSettingReader_->read( Units::DEGREE, eastHdgSetting_ );
    westHdgSettingReader_->read( Units::DEGREE, westHdgSetting_ );
    nEHdgSettingReader_->read( Units::DEGREE, nEHdgSetting_ );
    nWHdgSettingReader_->read( Units::DEGREE, nWHdgSetting_ );
    sEHdgSettingReader_->read( Units::DEGREE, sEHdgSetting_ );
    sWHdgSettingReader_->read( Units::DEGREE, sWHdgSetting_ );
    searchHdgSettingReader_->read( Units::DEGREE, searchHdgSetting_ );
    lonWestWallSettingReader_->read( Units::DEGREE, lonWestWallSetting_ );
    lonEastWallSettingReader_->read( Units::DEGREE, lonEastWallSetting_ );
    latNorthWallSettingReader_->read( Units::DEGREE, latNorthWallSetting_ );
    latSouthWallSettingReader_->read( Units::DEGREE, latSouthWallSetting_ );
    shapeZSettingReader_->read( shapeZSetting_ );
    shapeNSettingReader_->read( shapeNSetting_ );
    flagNorthSouthSweepSettingReader_->read( flagNorthSouthSweepSetting_ );
    flagNorthwardSweepSettingReader_->read( flagNorthwardSweepSetting_ );
    flagEastwardSweepSettingReader_->read( flagEastwardSweepSetting_ );
    waitSecondsSettingReader_->read( Units::SECOND, waitSecondsSetting_ );
}

/// Initialize function
void FrontTracking::initialize( void )
{
    logger_.syslog( "Initialize FrontTrackingComponent.", Syslog::INFO );

    if( !searchHdgSettingReader_->isActive() )
    {
        logger_.syslog( "Missing initial search heading setting.", Syslog::ERROR );
        initialized_ = false;
        return;
    }

    //debug
    //logger_.syslog("waitSecondsSettingReader_->isActive() = " + Str(waitSecondsSettingReader_->isActive()), Syslog::INFO);

    if( !waitSecondsSettingReader_->isActive() )
    {
        logger_.syslog( "Missing setting of continued-flight duration posterior to front detection.", Syslog::ERROR );
        initialized_ = false;
        return;
    }

    if( !latitudeReader_->isActive() || !longitudeReader_->isActive() )
    {
        initialized_ = false;
        return;
    }

    // Read settings
    readSettings();

    headingReader_->requestData( true );

    // Argument output variables
    frontDetectionWriter_ = findArgWriter( FrontTrackingIF::FRONT_DETECTION );

    initialized_ = true;
}

bool FrontTracking::isSatisfied()
{
    return false;
}

void FrontTracking::run()
{
    //debug
    //logger_.syslog( "CALLING RUN", Syslog::INFO ); // *** DEBUG
    doRun();
}

bool FrontTracking::runIfUnsatisfied()
{
    bool satisfied( false );
    if( readParams() )
    {
        //debug
        //logger_.syslog( "CALLING doRun in runIfUnsatisfied", Syslog::INFO ); // *** DEBUG

        doRun();
    }
    return satisfied;
}

bool FrontTracking::readParams()
{
    if( !initialized_ )
    {
        initialize();
        return false;
    }

    if( !latitudeReader_->isActive() || !longitudeReader_->isActive() )
    {
        latitudeReader_->requestData( true );
        longitudeReader_->requestData( true );
        return false;
    }

    latitudeReader_->read( Units::DEGREE, latitude_ );
    longitudeReader_->read( Units::DEGREE, longitude_ );

    if( isnan( latitude_ ) || isnan( longitude_ ) )
    {
        logger_.syslog( "Location is nan.", Syslog::ERROR );
        return false;
    }

    if( !headingReader_->isActive() )
    {
        headingReader_->requestData( true );
        return false;
    }

    headingReader_->read( Units::DEGREE, heading_ );

    if( isnan( heading_ ) )
    {
        logger_.syslog( "Heading is nan.", Syslog::ERROR );
        return false;
    }

    frontReader_->read( frontDetected_ );

    /* debug

    // debug. Mock front 1.
    if( latitude_ > 36.81 && !flagtmp1_ )
    {
        frontDetected_ = true;
        flagtmp1_ = true;
        logger_.syslog( "Mock front 1 detected.", Syslog::IMPORTANT );
    }

    // debug. Mock front 2.
    if( latitude_ > 36.812 && !flagtmp2_ )
    {
        frontDetected_ = true;
        flagtmp2_ = true;
        logger_.syslog( "Mock front 2 detected.", Syslog::IMPORTANT );
    }

    // debug. Mock front 3.
    if( latitude_ > 36.813 && !flagtmp3_ )
    {
        frontDetected_ = true;
        flagtmp3_ = true;
        logger_.syslog( "Mock front 3 detected.", Syslog::IMPORTANT );
    }

    // debug. Mock front 4.
    if( latitude_ > 36.82 && !flagtmp4_ )
    {
        frontDetected_ = true;
        flagtmp4_ = true;
        logger_.syslog( "Mock front 4 detected.", Syslog::IMPORTANT );
    }
    */

    if( NULL != frontDetectionWriter_ )
    {
        Timestamp now( Timestamp::Now() );
        frontDetectionWriter_->write( Units::BOOL, frontDetected_, now );
    }

    return true;
}

///
void FrontTracking::doRun()
{
    int headingMode;
    headingMode = HorizontalControlIF::HEADING;
    horizontalModeWriter_->write( Units::ENUM, headingMode );

    headingCmdWriter_->write( Units::DEGREE, searchHdgSetting_ );

    /* debug
    logger_.syslog("shapeZSetting_, flagNorthwardSweepSetting_ = "
    	   + Str(shapeZSetting_) + ", " + Str(flagNorthwardSweepSetting_),
    	   Syslog::INFO ); // DEBUG

    logger_.syslog("longitude_, lonWestWallSetting_  = "
    	   + Str(longitude_) + ", " + Str(lonWestWallSetting_),
    	   Syslog::INFO ); // DEBUG

    logger_.syslog("heading_, searchHdgSetting_ = "
    	   + Str(heading_) + ", " + Str(searchHdgSetting_),
    	   Syslog::INFO ); // DEBUG
    */

    // Logic goes here for changing the heading and informs calc satisfied

    if( hitWallURI_ )
    {
        hitWallURI_ = false;
        hitWallWriter_->write( Units::BOOL, hitWallURI_, dataTimestamp_ );
    }

    if( validFrontDetectedURI_ )
    {
        validFrontDetectedURI_ = false;
        validFrontDetectedWriter_->write( Units::BOOL, validFrontDetectedURI_, dataTimestamp_ );
    }

    // Hit east wall.
    if( ( longitude_ >= lonEastWallSetting_ ) && ( ( heading_ > 0.0 ) && ( heading_ < 180.0 ) ) && hitWallEnable_ )
    {
        searchHdgSettingPrevious_ = searchHdgSetting_;

        if( frontDetectionTime_.elapsed() >= waitSecondsSetting_ ) // AUV has completed post-front-detection continued flight
        {
            hitWallURI_ = true;
            hitWallWriter_->write( Units::BOOL, hitWallURI_, dataTimestamp_ );
            hitEastWall_ = true;
            hitWallEnable_ = false;
        }

        /* debug
        logger_.syslog("longitude_, lonWestWallSetting_, lonEastWallSetting_, latitude_, latNorthWallSetting_, latSouthWallSetting_, flagNorthwardSweepSetting_ = "
        	       + Str(longitude_) + ", " + Str(lonWestWallSetting_) + ", " + Str(lonEastWallSetting_) + ", "
        	       + Str(latitude_) + ", " + Str(latNorthWallSetting_) + ", " + Str(latSouthWallSetting_) + ", "
        	       + Str(flagNorthwardSweepSetting_),
        	       Syslog::INFO ); // DEBUG
        */

        if( flagNorthSouthSweepSetting_ )
        {
            if( flagNorthwardSweepSetting_ )
            {
                if( shapeZSetting_ )
                    searchHdgSetting_ = westHdgSetting_;
                else
                    searchHdgSetting_ = nWHdgSetting_;

                logger_.syslog( "Hit east wall on northward sweep. Continue northward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
            }
            else
            {
                if( shapeZSetting_ )
                    searchHdgSetting_ = sWHdgSetting_;
                else
                    searchHdgSetting_ = westHdgSetting_;

                logger_.syslog( "Hit east wall on southward sweep. Continue southward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );

            }
        }
        else
        {
            if( shapeNSetting_ )
                searchHdgSetting_ = nWHdgSetting_;
            else
                searchHdgSetting_ = sWHdgSetting_;

            flagEastwardSweepSetting_ = false;

            logger_.syslog( "Hit east wall on east-west sweep. Bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
        }

        //debug
        //logger_.syslog("Actual heading = " + Str(heading_) + " degrees", Syslog::INFO ); // DEBUG
    }

    // Hit west wall. Covers the cases heading reading is positive or negative.
    if( ( longitude_ <= lonWestWallSetting_ ) && ( ( ( heading_ > 180.0 ) && ( heading_ < 360.0 ) ) || ( ( heading_ > -180.0 ) && ( heading_ < 0.0 ) ) ) && hitWallEnable_ )

    {
        searchHdgSettingPrevious_ = searchHdgSetting_;

        if( frontDetectionTime_.elapsed() >= waitSecondsSetting_ ) // AUV has completed post-front-detection continued flight
        {
            hitWallURI_ = true;
            hitWallWriter_->write( Units::BOOL, hitWallURI_, dataTimestamp_ );
            hitWestWall_ = true;
            hitWallEnable_ = false;
        }

        /* debug
        logger_.syslog("longitude_, lonWestWallSetting_, lonEastWallSetting_, latitude_, latNorthWallSetting_, latSouthWallSetting_, flagNorthwardSweepSetting_ = "
        	       + Str(longitude_) + ", " + Str(lonWestWallSetting_) + ", " + Str(lonEastWallSetting_) + ", "
        	       + Str(latitude_) + ", " + Str(latNorthWallSetting_) + ", " + Str(latSouthWallSetting_) + ", "
        	       + Str(flagNorthwardSweepSetting_),
        	       Syslog::INFO ); // DEBUG
        */

        if( flagNorthSouthSweepSetting_ )
        {
            if( flagNorthwardSweepSetting_ )
            {
                if( shapeZSetting_ )
                    searchHdgSetting_ = nEHdgSetting_;
                else
                    searchHdgSetting_ = eastHdgSetting_;

                logger_.syslog( "Hit west wall on northward sweep. Continue northward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
            }
            else
            {
                if( shapeZSetting_ )
                    searchHdgSetting_ = eastHdgSetting_;
                else
                    searchHdgSetting_ = sEHdgSetting_;

                logger_.syslog( "Hit west wall on southward sweep. Continue southward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
            }
        }
        else
        {
            if( shapeNSetting_ )
                searchHdgSetting_ = sEHdgSetting_;
            else
                searchHdgSetting_ = nEHdgSetting_;

            flagEastwardSweepSetting_ = true;

            logger_.syslog( "Hit west wall on east-west sweep. Bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
        }

        //debug
        //logger_.syslog("Actual heading = " + Str(heading_) + " degrees", Syslog::INFO ); // DEBUG
    }

    // Hit north wall. Covers the cases heading reading is positive or negative.
    if( ( latitude_ >= latNorthWallSetting_ ) && ( ( ( heading_ > 270.0 ) && ( heading_ < 360.0 ) ) || ( ( heading_ > 0.0 ) && ( heading_ < 90.0 ) ) || ( ( heading_ > -90.0 ) && ( heading_ < 0.0 ) ) ) && hitWallEnable_ )
    {
        searchHdgSettingPrevious_ = searchHdgSetting_;

        if( frontDetectionTime_.elapsed() >= waitSecondsSetting_ ) // AUV has completed post-front-detection continued flight
        {
            hitWallURI_ = true;
            hitWallWriter_->write( Units::BOOL, hitWallURI_, dataTimestamp_ );
            hitNorthWall_ = true;
            hitWallEnable_ = false;
        }

        /* debug
        logger_.syslog("longitude_, lonWestWallSetting_, lonEastWallSetting_, latitude_, latNorthWallSetting_, latSouthWallSetting_, flagNorthwardSweepSetting_ = "
        	       + Str(longitude_) + ", " + Str(lonWestWallSetting_) + ", " + Str(lonEastWallSetting_) + ", "
        	       + Str(latitude_) + ", " + Str(latNorthWallSetting_) + ", " + Str(latSouthWallSetting_) + ", "
        	       + Str(flagNorthwardSweepSetting_),
        	       Syslog::INFO ); // DEBUG
        */

        if( flagNorthSouthSweepSetting_ )
        {
            if( shapeZSetting_ )
                searchHdgSetting_ = sWHdgSetting_;
            else
                searchHdgSetting_ = sEHdgSetting_;

            flagNorthwardSweepSetting_ = false;

            logger_.syslog( "Hit north wall on north-south sweep. Bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
        }
        else
        {
            if( flagEastwardSweepSetting_ )
            {
                if( shapeNSetting_ )
                    searchHdgSetting_ = sEHdgSetting_;
                else
                    searchHdgSetting_ = southHdgSetting_;

                logger_.syslog( "Hit north wall on eastward sweep. Continue eastward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
            }
            else
            {
                if( shapeNSetting_ )
                    searchHdgSetting_ = southHdgSetting_;
                else
                    searchHdgSetting_ = sWHdgSetting_;

                logger_.syslog( "Hit north wall on westward sweep. Continue westward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
            }
        }

        //debug
        //logger_.syslog("Actual heading = " + Str(heading_) + " degrees", Syslog::INFO ); // DEBUG
    }

    // Hit south wall. Covers the cases heading reading is positive or negative.
    if( ( latitude_ <= latSouthWallSetting_ ) && ( ( ( heading_ > 90.0 ) && ( heading_ < 180.0 ) ) || ( ( heading_ > 180.0 ) && ( heading_ < 270.0 ) ) || ( ( heading_ > -180.0 ) && ( heading_ < -90.0 ) ) ) && hitWallEnable_ )
    {
        searchHdgSettingPrevious_ = searchHdgSetting_;

        if( frontDetectionTime_.elapsed() >= waitSecondsSetting_ ) // AUV has completed post-front-detection continued flight
        {
            hitWallURI_ = true;
            hitWallWriter_->write( Units::BOOL, hitWallURI_, dataTimestamp_ );
            hitSouthWall_ = true;
            hitWallEnable_ = false;
        }

        /* debug
        logger_.syslog("longitude_, lonWestWallSetting_, lonEastWallSetting_, latitude_, latNorthWallSetting_, latSouthWallSetting_, flagNorthwardSweepSetting_, = "
        	       + Str(longitude_) + ", " + Str(lonWestWallSetting_) + ", " + Str(lonEastWallSetting_) + ", "
        	       + Str(latitude_) + ", " + Str(latNorthWallSetting_) + ", " + Str(latSouthWallSetting_) + ", "
        	       + Str(flagNorthwardSweepSetting_),
        	       Syslog::INFO ); // DEBUG
        */

        if( flagNorthSouthSweepSetting_ )
        {
            if( shapeZSetting_ )
                searchHdgSetting_ = nEHdgSetting_;
            else
                searchHdgSetting_ = nWHdgSetting_;

            flagNorthwardSweepSetting_ = true;

            logger_.syslog( "Hit south wall on north-south sweep. Bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
        }
        else
        {
            if( flagEastwardSweepSetting_ )
            {
                if( shapeNSetting_ )
                    searchHdgSetting_ = northHdgSetting_;
                else
                    searchHdgSetting_ = nEHdgSetting_;

                logger_.syslog( "Hit south wall on eastward sweep. Continue eastward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
            }
            else
            {
                if( shapeNSetting_ )
                    searchHdgSetting_ = nWHdgSetting_;
                else
                    searchHdgSetting_ = northHdgSetting_;

                logger_.syslog( "Hit south wall on westward sweep. Continue westward sweep but bounce back to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
            }
        }

        //debug
        //logger_.syslog("Actual heading = " + Str(heading_) + " degrees", Syslog::INFO ); // DEBUG
    }

    // Enable flagging hitwall.
    if( !hitWallEnable_ &&
            ( ( hitEastWall_ && !( ( heading_ > 0.0 ) && ( heading_ < 180.0 ) ) ) ||
              ( hitWestWall_ && !( ( ( heading_ > 180.0 ) && ( heading_ < 360.0 ) ) || ( ( heading_ > -180.0 ) && ( heading_ < 0.0 ) ) ) ) ||
              ( hitNorthWall_ && !( ( ( heading_ > 270.0 ) && ( heading_ < 360.0 ) ) || ( ( heading_ > 0.0 ) && ( heading_ < 90.0 ) ) || ( ( heading_ > -90.0 ) && ( heading_ < 0.0 ) ) ) ) ||
              ( hitSouthWall_ && !( ( ( heading_ > 90.0 ) && ( heading_ < 180.0 ) ) || ( ( heading_ > 180.0 ) && ( heading_ < 270.0 ) ) || ( ( heading_ > -180.0 ) && ( heading_ < -90.0 ) ) ) ) ) )
    {
        hitWallEnable_ = true;

        if( hitEastWall_ )
            hitEastWall_ = false;

        if( hitWestWall_ )
            hitWestWall_ = false;

        if( hitNorthWall_ )
            hitNorthWall_ = false;

        if( hitSouthWall_ )
            hitSouthWall_ = false;
    }

    if( frontDetected_ )
    {
        frontDetected_ = false;
        cntFrontDetection_++;

        //debug
        //logger_.syslog("cntFrontDetection_ = " + Str(cntFrontDetection_), Syslog::INFO ); // DEBUG

        if( ( cntFrontDetection_ == 1 ) ||
                ( ( cntFrontDetection_ >= 2 ) &&
                  ( frontDetectionTime_.elapsed() >= waitSecondsSetting_ ) ) )
        {
            if( cntIgnoreDetection_ >= 1 )
            {
                cntIgnoreDetection_--;

                logger_.syslog( "Front detected, but ignored to match the preceding ignored detection.", Syslog::IMPORTANT );

                //debug
                //logger_.syslog("cntIgnoreDetection_ = " + Str(cntIgnoreDetection_), Syslog::INFO );
            }
            else
            {
                frontDetectionTime_ = Timestamp::Now();
                validFrontDetectedURI_ = true;
                validFrontDetectedWriter_->write( Units::BOOL, validFrontDetectedURI_, dataTimestamp_ );
                logger_.syslog( "Front detected. Continue flight for " + Str( waitSecondsSetting_ ) + " seconds on heading " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );

                searchHdgSettingPrevious_ = searchHdgSetting_;
                pastFrontNewHdgEnable_ = true;

                pastFrontNewSearchHdg_ = searchHdgSetting_;

                if( flagNorthSouthSweepSetting_ )
                {
                    if( flagNorthwardSweepSetting_ )
                    {
                        if( fabs( searchHdgSetting_ - nEHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = westHdgSetting_;

                        if( fabs( searchHdgSetting_ - nWHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = eastHdgSetting_;

                        if( fabs( searchHdgSetting_ - westHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = nEHdgSetting_;

                        if( fabs( searchHdgSetting_ - eastHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = nWHdgSetting_;
                    }
                    else
                    {
                        if( fabs( searchHdgSetting_ - sEHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = westHdgSetting_;

                        if( fabs( searchHdgSetting_ - sWHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = eastHdgSetting_;

                        if( fabs( searchHdgSetting_ - westHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = sEHdgSetting_;

                        if( fabs( searchHdgSetting_ - eastHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = sWHdgSetting_;
                    }
                }

                if( !flagNorthSouthSweepSetting_ )
                {
                    if( flagEastwardSweepSetting_ )
                    {
                        if( fabs( searchHdgSetting_ - nEHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = southHdgSetting_;

                        if( fabs( searchHdgSetting_ - sEHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = northHdgSetting_;

                        if( fabs( searchHdgSetting_ - northHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = sEHdgSetting_;

                        if( fabs( searchHdgSetting_ - southHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = nEHdgSetting_;
                    }
                    else
                    {
                        if( fabs( searchHdgSetting_ - nWHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = southHdgSetting_;

                        if( fabs( searchHdgSetting_ - sWHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = northHdgSetting_;

                        if( fabs( searchHdgSetting_ - northHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = sWHdgSetting_;

                        if( fabs( searchHdgSetting_ - southHdgSetting_ ) < 1.0 )
                            pastFrontNewSearchHdg_ = nWHdgSetting_;
                    }
                }
            }
        }
        else
        {
            cntIgnoreDetection_++;

            logger_.syslog( "Front detected, but ignored because the last detection's continuation flight time is not over yet.", Syslog::IMPORTANT );

            //debug
            //logger_.syslog("cntIgnoreDetection_ = " + Str(cntIgnoreDetection_), Syslog::INFO );
        }
    }

    if( pastFrontNewHdgEnable_ &&
            ( frontDetectionTime_.elapsed() >= waitSecondsSetting_ ) )
    {
        pastFrontNewHdgEnable_ = false;
        searchHdgSetting_ = pastFrontNewSearchHdg_;
        logger_.syslog( "Continued flight (after front detection) completed. searchHdgSetting_ switches from " + Str( searchHdgSettingPrevious_ ) + " degrees to " + Str( searchHdgSetting_ ) + " degrees.", Syslog::IMPORTANT );
    }
}

/// Uninit function
void FrontTracking::uninitialize( void )
{
    logger_.syslog( "Uninitialize FrontTrackingComponent." );
    headingReader_->requestData( false );
    initialized_ = false;
}


/// Mission Component factory interface
Behavior* FrontTracking::CreateBehavior( const Str& prefix, const Module* module )
{
    return new FrontTracking( prefix, module );
}