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

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

#include "InternalSim.h"

#include "controlModule/VerticalControlIF.h"
#include "data/ConfigReader.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 "InternalSimIF.h"
#include "controlModule/VerticalControlIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "controlModule/SpeedControlIF.h"

//=== Constructor/destructor ==
/// Constructor
InternalSim::InternalSim( const Module* module )
    : SyncSimulatorComponent( InternalSimIF::NAME, module ),
      simulator_( false, 2 ),
      ok_( false ),
      haveFix_( false ),
      gotFirstFix_( false ),
      surfaceThreshold_( 1.0 )
{

    double badAccuracy( 1.0e30 );

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

    // Ahrs slate readers and writers
    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 ) );
    headingWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ORIENTATION, Units::DEGREE, badAccuracy );
    pitchWriter_ = newUniversalWriter( UniversalURI::PLATFORM_PITCH_ANGLE, Units::DEGREE, badAccuracy );
    rollWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ROLL_ANGLE,  Units::DEGREE, badAccuracy );

    // Depth slate readers and writers
    depthReader_ = newUniversalReader( UniversalURI::DEPTH ); //, this, Units::METER( 0.0 ) );
    depthWriter_ = newUniversalWriter( UniversalURI::DEPTH, Units::METER, badAccuracy );

    // GPS slate readers and writers
    latitudeFixReader_ = newUniversalReader( UniversalURI::LATITUDE_FIX );
    longitudeFixReader_ = newUniversalReader( UniversalURI::LONGITUDE_FIX );
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

    latitudeWriter_ = newUniversalWriter( UniversalURI::LATITUDE, Units::DEGREE, badAccuracy );
    longitudeWriter_ = newUniversalWriter( UniversalURI::LONGITUDE, Units::DEGREE, badAccuracy );

    // Tailcone slate readers and writers
    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 ) );
    propOmegaWriter_ = newUniversalWriter( UniversalURI::PLATFORM_PROPELLER_ROTATION_RATE, Units::RADIAN_PER_SECOND, badAccuracy );
    elevatorAngleWriter_ = newUniversalWriter( UniversalURI::PLATFORM_ELEVATOR_ANGLE, Units::DEGREE, badAccuracy );
    rudderAngleWriter_ = newUniversalWriter( UniversalURI::PLATFORM_RUDDER_ANGLE, Units::DEGREE, badAccuracy );
    massPositionWriter_ = newUniversalWriter( UniversalURI::PLATFORM_MASS_POSITION, Units::METER, badAccuracy );

    // Configuration
    surfaceThresholdCfgReader_    = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG );

    setAllowableFailures( 999 );
    setRetryTimeout( Timespan( 1 ) );
}

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

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

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

    ok_ = loadParams();
    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine.\n" ), Syslog::CRITICAL );
    }

    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 InternalSim::run( void )
{
    if( !ok_ )
    {
        return ;
    }

    float depth;
    depthReader_->read( Units::METER, depth );

    if( !gotFirstFix_ )
    {
        if( latitudeFixReader_->getTimestamp().elapsed() <= dt_ )
        {
            gotFirstFix_ = true;
        }
    }

    surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ );

    if( gotFirstFix_ && ( ( latitudeFixReader_->getTimestamp().elapsed() <= dt_ ) || ( depth < surfaceThreshold_ ) ) )
    {
        double latitude;
        double longitude;
        float heading;
        float pitch;
        float roll;

        // Set the depth
        if( !isnan( depth ) )
        {
            simulator_.setDepth( depth );
        }

        // TODO: Should this really set the lat/lon from latitude and longitude? or should it be from latitude_fix and longitude_fix?
        // Set lat lon
        if( latitudeReader_->read( Units::RADIAN, latitude ) && longitudeReader_->read( Units::RADIAN, longitude ) )
        {
            if( !isnan( latitude ) && !isnan( longitude ) )
            {
                long error = simulator_.setLocation( latitude, longitude );
                if( error != 0 )
                {
                    logger_.syslog( "Error on conversion of position fix to UTM: ", Wgs84::UtmErrorToString( error ), Syslog::ERROR );
                    setFailure( FailureMode::SOFTWARE );
                    return;
                }
            }
            haveFix_ = true;
        }

        // Set the heading
        if( headingReader_->read( Units::RADIAN, heading ) )
        {
            if( !isnan( heading ) )
            {
                simulator_.setHeading( heading );
            }
        }

        if( pitchReader_->read( Units::RADIAN, pitch ) )
        {
            simulator_.setPitch( pitch );
        }

        if( rollReader_->read( Units::RADIAN, roll ) )
        {
            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_ );
    runParams_.dt_ = dt_.asDouble();
    if( runParams_.dt_ <= 0 )
    {
        runParams_.dt_ = 0.4;
    }
    if( runParams_.dt_ > 5.0 )
    {
        runParams_.dt_ = 5.0;
    }
    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 );
        setFailure( FailureMode::SOFTWARE );
    }
    else
    {
        publishState();
    }

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

}

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

/// Load VerticalControl parameters
bool InternalSim::loadParams( void )
{
    // Check if all the parameters are read correctly
    bool ok = true;
    return ok;
}


void InternalSim::publishState()
{
    // Tailcone outputs
    propOmegaWriter_->write( Units::RADIAN_PER_SECOND, results_.propOmega_ );
    elevatorAngleWriter_->write( Units::RADIAN, results_.elevatorAngle_ );
    rudderAngleWriter_->write( Units::RADIAN, results_.rudderAngle_ );
    massPositionWriter_->write( Units::RADIAN, results_.massPosition_ );

    // AHRS outputs
    if( !isnan( results_.heading_ ) )
    {
        headingWriter_->write( Units::RADIAN, results_.heading_ );
    }
    if( !isnan( results_.pitch_ ) )
    {
        pitchWriter_->write( Units::RADIAN, results_.pitch_ );
    }
    if( !isnan( results_.roll_ ) )
    {
        rollWriter_->write( Units::RADIAN, results_.roll_ );
    }

    // Depth outputs
    if( !isnan( results_.depth_ ) )
    {
        depthWriter_->write( Units::METER, results_.depth_ );
    }

    // GPS outputs
    if( haveFix_ )
    {
        if( !isnan( results_.latitudeDeg_ ) && !isnan( results_.longitudeDeg_ ) )
        {
            latitudeWriter_->write( Units::DEGREE, results_.latitudeDeg_ );
            longitudeWriter_->write( Units::DEGREE, results_.longitudeDeg_ );
        }
    }
}