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

#include "NavChart.h"
#include "NavChartIF.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "io/NavChartDb.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "utils/Timestamp.h"

NavChart::NavChart( const Module* module )
    : SyncNavigationComponent( NavChartIF::NAME, module ),
      useChartsForAltitude_( 0 )
{
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    latitudeFixReader_ = newUniversalReader( UniversalURI::LATITUDE_FIX );
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    seaFloorDepthWriter_ = newUniversalWriter( UniversalURI::SEA_FLOOR_DEPTH_BELOW_GEOID, Units::METER, 250.0 );
    heightAboveSeaFloorWriter_ = newUniversalWriter( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR, Units::METER, 250.0 );
    offshoreDistanceWriter_ = newUniversalWriter( UniversalURI::DISTANCE_FROM_SHORE, Units::KILOMETER, 2.0 );
    nonUniversalHeightAboveSeaFloorWriter_ = newDataWriter( NavChartIF::HEIGHT_ABOVE_SEA_FLOOR_COMP );
    useChartAltitudeReader_ = newConfigReader( NavChartIF::USE_CHART_ALTITUDE );
}

NavChart::~NavChart()
{
}

void NavChart::initialize( void )
{
    logger_.syslog( "Initialize NavChart Navigation." );

    // Read in configuration paramaters
    if( !( useChartAltitudeReader_->read( Units::BOOL, useChartsForAltitude_ ) ) )
    {
        logger_.syslog( "Did not read configuration variables", Syslog::FAULT );
    }

}

void NavChart::run( void )
{
    NavChartDb* navChartDb = NavChartDb::GetInstance();
    if( NULL == navChartDb || !navChartDb->ready() )
    {
        return;
    }
    if( !latitudeReader_->isActive() || !longitudeReader_->isActive() )
    {
        return;
    }
    double latitude = nanf( "" );
    double longitude = nanf( "" );

    // Don't do anything with a lat/lon that doesn't exist...
    if( !( latitudeReader_->read( Units::RADIAN, latitude ) ) || !( longitudeReader_->read( Units::RADIAN, longitude ) ) )
    {
        return;
    }

    // Don't do anything with a lat/lon that's still nan at this point. We shouldn't really be able to get to this line if
    // the read function is working correctly up above.
    if( isnan( latitude ) || isnan( longitude ) )
    {
        return;
    }

    navChartDb->updateCurrent( latitude, longitude, gotFix() );
    float seaFloorDepth = navChartDb->getSeaFloorDepth( latitude, longitude );
    if( !isnan( seaFloorDepth ) )
    {
        seaFloorDepthWriter_->write( Units::METER, seaFloorDepth );
        if( depthReader_->isActive() )
        {
            float depth( nanf( " " ) );
            if( depthReader_->read( Units::METER, depth ) && !isnan( depth ) )
            {
                // If the code below seems strange, read on. Writing height above sea floor to the universal as a calculated value (as seen above) can
                // be dangerous if a measured value is being written (e.g. from a DVL or altimeter) and the measured value drops out. Specifically it's
                // quite dangerous if you're in varied terrain or the ded reckoned nav isn't very accurate and the switch from measured to calculated
                // results in a big change in height above sea floor. This has been known to cause the vehicle to go diving into the bottom in the past.
                // The reason for writing to a "local" component level writer is in case the users want to see what the chart thinks altitude would be
                // in a given area of the chart.
                if( useChartsForAltitude_ )
                {
                    heightAboveSeaFloorWriter_->write( Units::METER, seaFloorDepth - depth );
                }
                else
                {
                    // Just log the component version of this writer.
                    nonUniversalHeightAboveSeaFloorWriter_->write( Units::METER, seaFloorDepth - depth );
                }

            }
            // This is a simple test to see if the Sim is running
            double latDeg;
            if( SimSlate::Read( SimSlate::LATITUDE_DEGREE, latDeg ) )
            {
                if( !isnan( depth ) )
                {
                    SimSlate::Write( SimSlate::ALTITUDE_METER, seaFloorDepth - depth );
                }
            }
            else
            {
                SimSlate::Clear( SimSlate::ALTITUDE_METER );
            }
        }
    }

    float distanceFromShore( navChartDb->getDistanceFromShore( latitude, longitude ) );
    if( !isnan( distanceFromShore ) )
    {
        offshoreDistanceWriter_->write( Units::METER, distanceFromShore );
    }
}

bool NavChart::gotFix()
{
    if( latitudeFixReader_->getTimestamp() > lastFixTime_ )
    {
        lastFixTime_ = latitudeFixReader_->getTimestamp();
        return true;
    }
    return false;
}

/// Uninit function
void NavChart::uninitialize( void )
{
    logger_.syslog( "Uninitialize NavChart Navigation." );
}