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

#include <math.h>
#include <cstdio>

#include "SimSlate.h"
#include "SimACommsRangeHandler.h"
#include "SimACommsDataHandler.h"


SimSlate::AhrsEntry SimSlate::ahrsEntry_;
SimSlate::BatteryEntry SimSlate::batteryEntry_;
SimSlate::DvlEntry SimSlate::dvlEntry_;

double SimSlate::Entries_[ NUM_ENTRIES ];

SimSlate::StaticInitializer SimSlate::StaticInitializer_;

SimSlate::StaticInitializer::StaticInitializer()
{
    for( int i = 0; i < NUM_ENTRIES; ++i )
    {
        Clear( ( SimEntryType )i );
    }
}

bool SimSlate::Clear( SimEntryType simEntryType )
{
    if( simEntryType < 0 || simEntryType >= NUM_ENTRIES )
    {
        return false;
    }
    Entries_[ simEntryType ] = nan( "" );
    return true;
}

FastMap<const Str, SimSlate::ScienceEntry*> SimSlate::ScienceEntries_;

void SimSlate::Uninitialize()
{
    ScienceEntries_.clear( true );
}

template<typename T>
bool SimSlate::Read( SimEntryType simEntryType, T& value )
{
    if( simEntryType < 0 || simEntryType >= NUM_ENTRIES || isnan( Entries_[ simEntryType ] ) )
    {
        return false;
    }
    value = ( T )Entries_[ simEntryType ];
    return true;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template bool SimSlate::Read( SimEntryType, int& );
template bool SimSlate::Read( SimEntryType, float& );
template bool SimSlate::Read( SimEntryType, double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

bool SimSlate::ReadScience( const Str& name, const Unit& unit, float& value )
{
    ScienceEntry* entry = ScienceEntries_.get( name );
    if( NULL == entry )
    {
        value = nanf( "" );
        return false;
    }
    entry->get( unit, value );
    return true;
}

void SimSlate::Read( SimAhrsStruct& ahrs )
{
    ahrsEntry_.get( ahrs );
}

void SimSlate::Read( SimBatteryStruct& battery )
{
    batteryEntry_.get( battery );
}

void SimSlate::Read( SimDvlStruct& dvl )
{
    dvlEntry_.get( dvl );
}

template<typename T>
bool SimSlate::Write( SimEntryType simEntryType, const T& value )
{
    if( simEntryType < 0 || simEntryType >= NUM_ENTRIES )
    {
        return false;
    }
    Entries_[ simEntryType ] = ( double )value;
    return true;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template bool SimSlate::Write( SimEntryType, const int& );
template bool SimSlate::Write( SimEntryType, const float& );
template bool SimSlate::Write( SimEntryType, const double& );
/// \endcond DOXYGEN_CANT_HANDLE_THIS

bool SimSlate::WriteScience( const Str& name, const Unit& unit, const float value )
{
    ScienceEntry* entry = ScienceEntries_.get( name );
    if( NULL == entry )
    {
        entry = new ScienceEntry( unit, value );
        ScienceEntries_.put( name, entry );
    }
    else
    {
        entry->set( value );
    }
    return true;
}

void SimSlate::Write( const SimAhrsStruct& ahrs )
{
    ahrsEntry_.set( ahrs );
}

void SimSlate::Write( const SimBatteryStruct& battery )
{
    batteryEntry_.set( battery );
}

void SimSlate::Write( const SimDvlStruct& dvl )
{
    dvlEntry_.set( dvl );
}

void SimSlate::SendMessage( int destination, int source, const Str& message, float maxDistance, float delay, float mediumSpeed )
{
    SimACommsDataHandler::WriteOutgoingMessage( destination, source, message, maxDistance, delay, mediumSpeed );
}

bool SimSlate::ReceiveMessage( int& destination, int& source, Str& message )
{
    return SimACommsDataHandler::ReadIncomingMessage( destination, source, message );
}

bool SimSlate::SendRangeRequst( const int destination, const int source, const int numPings )
{
    return SimACommsRangeHandler::WriteOutgoingMessage( destination, source, numPings );
}

bool SimSlate::ReceiveRange( int& destination, float& range, float& azimuth, float& elevation, Timestamp& rxTime )
{
    return SimACommsRangeHandler::ReadIncomingMessage( destination, range, azimuth, elevation, rxTime );
}

void SimSlate::SetAcousticResponseTimeout( const Timespan& acousticResponseTimeout )
{
    SimACommsDataHandler::SetAcousticResponseTimeout( acousticResponseTimeout );
    SimACommsRangeHandler::SetAcousticResponseTimeout( acousticResponseTimeout );
}

void SimSlate::SetLocalAddress( const Str& vehicleName, int modemAddress )
{
    SimACommsDataHandler::SetLocalAddress( modemAddress );
    SimACommsRangeHandler::SetLocalAddress( vehicleName );
}

const char* SimSlate::ToString( SimEntryType simEntryType )
{
    switch( simEntryType )
    {
    case BUOYANCY_POSITION_CUBIC_METER:
        return "BUOYANCY_POSITION_CUBIC_METER";
    case DENSITY_KILOGRAM_PER_CUBIC_METER:
        return "DENSITY_KILOGRAM_PER_CUBIC_METER";
    case DEPTH_METER:
        return "DEPTH_METER";
    case ELEVATOR_ANGLE_RADIAN:
        return "ELEVATOR_ANGLE_RADIAN";
    case LATITUDE_DEGREE:
        return "LATITUDE_DEGREE";
    case LONGITUDE_DEGREE:
        return "LONGITUDE_DEGREE";
    case MAGNETIC_VARIATION_DEGREE:
        return "MAGNETIC_VARIATION_DEGREE";
    case MASS_CONCENTRATION_OF_CHLOROPHYLL_UG_PER_L:
        return "MASS_CONCENTRATION_OF_CHLOROPHYLL_UG_PER_L";
    case MASS_CONCENTRATION_OF_PETROLEUM_HYDROCARBON_KG_PER_CUBIC_METER:
        return "MASS_CONCENTRATION_OF_PETROLEUM_HYDROCARBON_KG_PER_CUBIC_METER";
    case MASS_CONCENTRATION_OF_OXYGEN_UG_PER_L:
        return "MASS_CONCENTRATION_OF_OXYGEN_UG_PER_L";
    case MASS_POSITION_METER:
        return "MASS_POSITION_METER";
    case MOLE_CONCENTRATION_OF_NITRATE_UMOLE_PER_L:
        return "MOLE_CONCENTRATION_OF_NITRATE_UMOLE_PER_L";
    case PROPELLER_OMEGA_RADIAN_PER_SECOND:
        return "PROPELLER_OMEGA_RADIAN_PER_SECOND";
    case RUDDER_ANGLE_RADIAN:
        return "RUDDER_ANGLE_RADIAN";
    case SALINITY_PART_PER_THOUSAND:
        return "SALINITY_PART_PER_THOUSAND";
    case SOUND_SPEED_METER_PER_SECOND:
        return "SOUND_SPEED_METER_PER_SECOND";
    case TEMPERATURE_DEGREE_CELSIUS:
        return "TEMPERATURE_DEGREE_CELSIUS";
    case HOMING_SENSOR_RANGE_M:
        return "HOMING_SENSOR_RANGE_M";
    case HOMING_SENSOR_AZIM_RAD:
        return "HOMING_SENSOR_AZIM_RAD";
    case HOMING_SENSOR_ELEV_RAD:
        return "HOMING_SENSOR_ELEV_RAD";
    default:
        return "(undefined)";
    }
};

SimSlate::ScienceEntry::ScienceEntry( const Unit& unit, float value )
    : unit_( unit ),
      value_( value )
{}

void SimSlate::ScienceEntry::set( float value )
{
    value_ = value;
}

void SimSlate::ScienceEntry::get( const Unit& unit, float& value )
{
    value = value_;
    if( unit == unit_ )
    {
        return;
    }
    unit_.toSI( value );
    unit.toScaled( value );
}

void SimSlate::AhrsEntry::set( const SimAhrsStruct& ahrsStruct )
{
    MutexLocker locker( ahrsMutex_ );

    ahrs_ = ahrsStruct;
}

void SimSlate::AhrsEntry::get( SimAhrsStruct& ahrsStruct )
{
    MutexLocker locker( ahrsMutex_ );

    ahrsStruct = ahrs_;
}

void SimSlate::BatteryEntry::set( const SimBatteryStruct& batteryStruct )
{
    MutexLocker locker( batteryMutex_ );

    battery_ = batteryStruct;
}

void SimSlate::BatteryEntry::get( SimBatteryStruct& batteryStruct )
{
    MutexLocker locker( batteryMutex_ );

    batteryStruct = battery_;
}

void SimSlate::DvlEntry::set( const SimDvlStruct& dvlStruct )
{
    MutexLocker locker( dvlMutex_ );

    dvl_ = dvlStruct;
}

void SimSlate::DvlEntry::get( SimDvlStruct& dvlStruct )
{
    MutexLocker locker( dvlMutex_ );

    dvlStruct = dvl_;
}