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

#include <math.h>
#include <cstdlib>

#include "NavigationSim.h"

#include "data/Location.h"
#include "data/Matrix6x6.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "simulatorModule/ParameterHandler.h"
#include "simulatorModule/SimulatorUtils.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
//#include "Tools/newmat-10D/newmatio.h"

// To get the relevant names for shared vars
#include "NavigationSimIF.h"
#include "controlModule/VerticalControlIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "controlModule/SpeedControlIF.h"

//=== Constructor/destructor ==
/// Constructor
NavigationSim::NavigationSim( const Module* module )
    : SyncDerivationComponent( NavigationSimIF::NAME, module ),
      simulator_( false, 2 ),
      ok_( false ),
      haveFix_( false )
{

    double badAccuracy( 1.0e5 );

    // Environmental readers
    densityReader_ = newUniversalReader( UniversalURI::SEA_WATER_DENSITY );

    // Ahrs slate readers
    headingReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION ); //, this, Units::RADIAN( 0.0 ) );
    pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE ); //, this, Units::RADIAN( 0.0 ) );
    rollReader_ = newUniversalReader( UniversalURI::PLATFORM_ROLL_ANGLE ); //, this, Units::RADIAN( 0.0 ) );

    // Depth slate reader
    depthReader_ = newUniversalReader( UniversalURI::DEPTH ); //, this, Units::METER( 0.0 ) );

    // GPS slate readers and writers
    latitudeFixReader_ = newUniversalReader( UniversalURI::LATITUDE_FIX );
    longitudeFixReader_ = newUniversalReader( UniversalURI::LONGITUDE_FIX );
    latitudeWriter_ = newUniversalWriter( UniversalURI::LATITUDE, Units::DEGREE, badAccuracy );
    longitudeWriter_ = newUniversalWriter( UniversalURI::LONGITUDE, Units::DEGREE, badAccuracy );
    speedWriter_ = newUniversalWriter( UniversalURI::PLATFORM_SPEED_WRT_SEA_WATER, Units::METER_PER_SECOND, badAccuracy );

    // Tailcone slate readers
    propOmegaActionReader_ = newDataReader( SpeedControlIF::PROP_OMEGA_ACTION ); //, this, Units::RADIAN_PER_SECOND( 0.0 ) );
    elevatorAngleActionReader_ = newDataReader( VerticalControlIF::ELEVATOR_ANGLE_ACTION ); //, this, Units::RADIAN( 0.0 ) );
    rudderAngleActionReader_ = newDataReader( HorizontalControlIF::RUDDER_ANGLE_ACTION ); //, this, Units::RADIAN( 0.0 ) );
    massPositionActionReader_ = newDataReader( VerticalControlIF::MASS_POSITION_ACTION ); //, this, Units::METER( 0.0 ) );
    propOmegaReader_ = newUniversalReader( UniversalURI::PLATFORM_PROPELLER_ROTATION_RATE ); //, this, Units::RADIAN_PER_SECOND( 0.0 ) );
    elevatorAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_ELEVATOR_ANGLE ); //, this, Units::RADIAN( 0.0 ) );
    rudderAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_RUDDER_ANGLE ); //, this, Units::RADIAN( 0.0 ) );
    massPositionReader_ = newUniversalReader( UniversalURI::PLATFORM_MASS_POSITION ); //, this, Units::METER( 0.0 ) );

}

/// Destructor
NavigationSim::~NavigationSim()
{}

/// Initialize
void NavigationSim::initialize( void )
{

    logger_.syslog( " NavigationSim initializing..." );

    SimInitStruct init;
    ok_ = SimulatorUtils::LoadInit( init, this, logger_ );

    if( ok_ )
    {
        *results_.errorMessage_ = 0;
        simulator_.initialize( init, results_ );
        if( *results_.errorMessage_ )
        {
            ok_ = false;
            logger_.syslog( "Simulator initialization error: ", results_.errorMessage_, Syslog::ERROR );
        }
        publishState();
    }
    else
    {
        logger_.syslog( "Unable to load simulator parameters from config files", Syslog::ERROR );
    }

}

/// Run
void NavigationSim::run( void )
{
    if( !ok_ )
    {
        return ;
    }

    if( latitudeFixReader_->getTimestamp().elapsed() <= dt_
            || longitudeFixReader_->getTimestamp().elapsed() <= dt_ )
    {
        double latitudeFix;
        double longitudeFix;
        latitudeFixReader_->read( Units::RADIAN, latitudeFix );
        longitudeFixReader_->read( Units::RADIAN, longitudeFix );
        long error = simulator_.setLocation( latitudeFix, longitudeFix );
        if( error != 0 )
        {
            logger_.syslog( "Error on conversion of position fix to UTM: ", Wgs84::UtmErrorToString( error ), Syslog::ERROR );
            return;
        }
        simulator_.setDepth( 0.0 );
        haveFix_ = true;
    }

    double depth, heading, pitch, roll;
    depthReader_->read( Units::METER, depth );
    headingReader_->read( Units::RADIAN, heading );
    pitchReader_->read( Units::RADIAN, pitch );
    rollReader_->read( Units::RADIAN, roll );

    simulator_.setDepth( depth );
    simulator_.setHeading( heading );
    simulator_.setPitch( pitch );
    simulator_.setRoll( roll );

    // These are things that should be loaded from the slate...

    // (rad/sec)
    propOmegaActionReader_->read( Units::RADIAN_PER_SECOND, runParams_.propOmegaAction_ );
    rudderAngleActionReader_->read( Units::RADIAN, runParams_.rudderAngleAction_ );
    elevatorAngleActionReader_->read( Units::RADIAN, runParams_.elevatorAngleAction_ );
    massPositionActionReader_->read( Units::METER, runParams_.massPositionAction_ );
    propOmegaReader_->read( Units::RADIAN_PER_SECOND, runParams_.propOmega_ );
    rudderAngleReader_->read( Units::RADIAN, runParams_.rudderAngle_ );
    elevatorAngleReader_->read( Units::RADIAN, runParams_.elevatorAngle_ );
    massPositionReader_->read( Units::METER, runParams_.massPosition_ );
    runParams_.dt_ = dt_.asDouble();
    if( runParams_.dt_ <= 0 )
    {
        runParams_.dt_ = 0.4;
    }
    densityReader_->read( Units::KILOGRAM_PER_CUBIC_METER, runParams_.density_ );

    *results_.errorMessage_ = 0;
    simulator_.run( runParams_, results_ );

    if( *results_.errorMessage_ )
    {
        logger_.syslog( "Simulator run error: ", results_.errorMessage_, Syslog::ERROR );
    }
    else
    {
        publishState();
    }

    // And that's it
    setState( BLOCK_NORMAL );

}

/// Deinitialize
void NavigationSim::uninitialize( void )
{}

void NavigationSim::publishState()
{
    if( haveFix_ )
    {
        if( !isnan( results_.latitudeDeg_ ) && !isnan( results_.longitudeDeg_ ) )
        {
            latitudeWriter_->write( Units::DEGREE, results_.latitudeDeg_ );
            longitudeWriter_->write( Units::DEGREE, results_.longitudeDeg_ );
        }
    }
    if( !isnan( results_.speed_ ) )
    {
        speedWriter_->write( Units::METER_PER_SECOND, results_.speed_ );
    }
}