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

#include "GobyModem.h"
#include "GobyModemIF.h"

#include "VehicleIF.h"
#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/UniversalDataWriter.h"
#include "supervisor/DataReceiver.h"
#include "supervisor/SendData.h"

#include <goby/acomms/dccl.h>
#include <goby/acomms/queue.h>
#include <goby/acomms/modem_driver.h>
#include <goby/acomms/amac.h>
#include <goby/acomms/bind.h>

#include "../../Build/Source/sensorModule/GobyModemMessage.pb.h"

void received_data_handler( const google::protobuf::Message& );
void received_ack_handler( const goby::acomms::protobuf::ModemTransmission&,
                           const google::protobuf::Message& );
void monitor_mac( const goby::acomms::protobuf::ModemTransmission& mac_msg );
void monitor_modem_receive( const goby::acomms::protobuf::ModemTransmission& rx_msg );

GobyModem* GobyModem::Instance_;

GobyModem::GobyModem( const Module* module )
    : SyncSensorComponent( GobyModemIF::NAME, module ),
      uart_( GobyModemIF::UART, GobyModemIF::BAUD, 0.5, logger_, 4095 ),
      loadControl_( GobyModemIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      ok_( true ),
      initialized_( false ),
      powerOnTimeStart_( Timestamp::Now() ), // Set timeout for start in case we've just restarted the application
      powerOnTimeout_( 10 ),
      vehicleId_( 0 ),
      modemType_(),
      networkIds_(),
      maxDistanceCfg_( 0 ),
      transBaudCfg_( 300 ),
      networkIdsChanged_( false ),
      pinging_( false )
{

    Instance_ = this;

    this->setAllowableFailures( 5 ); // Since the fail count is only reset after the completion of GPS/comms, it's possible that these can stack up. Will allow for 5 consecutive per "session".
    this->setRetryTimeout( 120 );

    platformConversationWriter_ = newUniversalWriter( UniversalURI::PLATFORM_CONVERSATION, Units::BOOL, 0 );

    vehicleIdReader_ = newConfigReader( VehicleIF::ID_CFG );
    modemTypeReader_ = newConfigReader( GobyModemIF::MODEM_TYPE_CFG );
    networkIdsReader_ = newConfigReader( GobyModemIF::NETWORK_IDS_CFG );
    maxDistanceReader_ = newConfigReader( GobyModemIF::MAX_DISTANCE_CFG );
    transBaudReader_ = newConfigReader( GobyModemIF::TRANS_BAUD_CFG );

    dusblPingCodeReader_ = newDataReader( GobyModemIF::DUSBL_PING_CODE_REQUESTED );

    mm_driver_ = NULL;
    q_manager_ = NULL;
    mac_ = NULL;

    // This configures the advanced run modes.
    setRunState( STOPPED );
    persistentPause_ = true;

    dccl_ = goby::acomms::DCCLCodec::get();
    dccl_->validate<GobyModemMessage>();

    //debugLevel_ = Syslog::INFO;

    DEBUG_LOG( "Construct" );
}

GobyModem::~GobyModem()
{
}

bool GobyModem::readConfig()
{
    bool ok = true;
    if( !simulateHardware() && NULL == mm_driver_ )
    {
        ok &= modemTypeReader_->read( modemType_ );
        if( ok )
        {
            if( modemType_ == "whoi_micromodem" )
            {
                mm_driver_ = new goby::acomms::MMDriver();
            }
            else if( modemType_ == "benthos_atm900" )
            {
                mm_driver_ = new goby::acomms::BenthosATM900Driver();
            }
            else
            {
                logger_.syslog( "Unknown modem type: " + modemType_, Syslog::CRITICAL );
                ok = false;
            }
        }
    }
    Str networkIds;
    ok &= networkIdsReader_->read( networkIds );
    if( ok && networkIds != networkIds_ )
    {
        networkIds_ = networkIds;
        networkIdsChanged_ = true;
    }
    ok &= vehicleIdReader_->read( Units::ENUM, vehicleId_ );
    ok &= maxDistanceReader_->read( Units::METER, maxDistanceCfg_ );
    ok &= transBaudReader_->read( Units::BIT_PER_SECOND, transBaudCfg_ );
    return ok;
}

void GobyModem::initialize()
{
    if( initialized_ )
    {
        return;
    }
    if( !readConfig() )
    {
        return;
    }
    DEBUG_LOG( Str( "Ini init, simulate=" ) + simulateHardware() );
    if( !simulateHardware() )
    {

        q_manager_ = new goby::acomms::QueueManager() ;
        mac_ = new goby::acomms::MACManager();
        //dccl_ = goby::acomms::DCCLCodec::get();

        // bind the signals of the goby objects
        bind( *mm_driver_, *q_manager_, *mac_ );

        // Enable xceiver
        uart_.enableUART();

        //
        // Initiate DCCL (libdccl)
        //
        goby::acomms::protobuf::DCCLConfig dccl_cfg;

        //dccl_->validate<GobyModemMessage>();

        //
        // Initiate queue manager (libqueue)
        //
        goby::acomms::protobuf::QueueManagerConfig q_manager_cfg;
        q_manager_cfg.set_modem_id( vehicleId_ );
        goby::acomms::protobuf::QueuedMessageEntry* q_entry = q_manager_cfg.add_message_entry();
        q_entry->set_protobuf_name( "GobyModemMessage" );

        goby::acomms::protobuf::QueuedMessageEntry::Role* src_role = q_entry->add_role();
        src_role->set_type( goby::acomms::protobuf::QueuedMessageEntry::SOURCE_ID );
        src_role->set_field( "source" );

        goby::acomms::protobuf::QueuedMessageEntry::Role* dest_role = q_entry->add_role();
        dest_role->set_type( goby::acomms::protobuf::QueuedMessageEntry::DESTINATION_ID );
        dest_role->set_field( "destination" );

        goby::acomms::connect( &q_manager_->signal_receive, &received_data_handler );
        goby::acomms::connect( &q_manager_->signal_ack, &received_ack_handler );

        //
        // Initiate modem driver (libmodemdriver)
        //
        goby::acomms::protobuf::DriverConfig driver_cfg;
        driver_cfg.set_modem_id( vehicleId_ );
        driver_cfg.set_serial_port( uart_.getDevice() );

        //
        // Initiate medium access control (libamac)
        //
        goby::acomms::protobuf::MACConfig mac_cfg;
        mac_cfg.set_type( goby::acomms::protobuf::MAC_FIXED_DECENTRALIZED );
        mac_cfg.set_modem_id( vehicleId_ );
        goby::acomms::connect( &mac_->signal_initiate_transmission, &monitor_mac );

        //
        // Start up everything
        //
        try
        {
            dccl_->set_cfg( dccl_cfg );
            q_manager_->set_cfg( q_manager_cfg );
            mac_->startup( mac_cfg );
            mm_driver_->startup( driver_cfg );
        }
        catch( std::runtime_error& e )
        {
            logger_.syslog( "exception at startup: ", e.what(), Syslog::CRITICAL );
        }
    }

    initialized_ = true;
}

void GobyModem::run()
{
}

void GobyModem::uninitialize()
{
    if( !initialized_ )
    {
        return;
    }

    if( !simulateHardware() )
    {
        try
        {
            mac_->shutdown();
            mm_driver_->shutdown();
        }
        catch( std::runtime_error& e )
        {
            logger_.syslog( "exception at shutdown: ", e.what(), Syslog::CRITICAL );
        }

        // unbind the signals of the goby objects
        unbind( *mm_driver_, *q_manager_, *mac_ );

        //delete dccl_; // Can't delete singleton
        delete mm_driver_;
        delete q_manager_;
        delete mac_;
    }
    initialized_ = false;
}

/// Do what needs to be done to run
/// Similar to initialize, in old init/run/uninit sequence
Component::RunState GobyModem::start()
{
    DEBUG_LOG( "Start" );

    initialize();

    ok_ = readConfig();
    if( !ok_ )
    {
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::CRITICAL );
        this->setFailure( FailureMode::SOFTWARE );
        return STOPPED;
    }

    // turn on power here
    DEBUG_LOG( "Powering up GobyModem" );
    if( !simulateHardware() )
    {
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::ERROR );
        }
    }

    powerOnTimeStart_ = Timestamp::Now();
    return STARTING;
}


/// Might follow a STOP...START sequence
Component::RunState GobyModem::starting()
{
    DEBUG_LOG( "Starting" );

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    if( powerOnTimeStart_.elapsed() > powerOnTimeout_ )
    {
        return RUNNABLE;
    }
    return STARTING;
}


/// Pause for a short period (indicated by pauseTime)
Component::RunState GobyModem::pause()
{
    DEBUG_LOG( "Pause" );

    // Last chance...
    if( isDataRequested() || isCommanded() )
    {
        return RUNNABLE;
    }

    // turn off power here
    DEBUG_LOG( "Powering down GobyModem" );
    if( !simulateHardware() )
    {
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::ERROR );
        }
    }

    return PAUSE;
}


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState GobyModem::paused()
{
    DEBUG_LOG( "Paused" );

    // See if it's time to get a fix
    if( isDataRequested() || isCommanded() )
    {
        DEBUG_LOG( "Powering up" );
        if( !simulateHardware() )
        {
            if( !loadControl_.powerUp() )
            {
                logger_.syslog( "Failed to power up", Syslog::ERROR );
                this->setFailure( FailureMode::HARDWARE );
            }
        }
        powerOnTimeStart_ = Timestamp::Now(); // Start the clock
        return STARTING;
    }
    return PAUSED;
}


Component::RunState GobyModem::resume()
{
    DEBUG_LOG( "Resume" );
    // turn on power here

    DEBUG_LOG( "Powering up GobyModem" );
    if( !simulateHardware() )
    {
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::ERROR );
        }
    }

    powerOnTimeStart_ = Timestamp::Now();
    return RESUMING; // State not used at this time
}


Component::RunState GobyModem::resuming()
{
    DEBUG_LOG( "Resuming" );
    return RUNNABLE; // State not used at this time
}


Component::RunState GobyModem::runnable()
{
    //DEBUG_LOG( "Runnable" );

    readConfig();
    if( networkIdsChanged_ )
    {
        networkIdsChanged_ = false;
        if( !simulateHardware() )
        {
            mac_->clear();
            int numIds = 0;
            Str* ids = networkIds_.split( numIds, "," );
            for( int i = 0; i < numIds; ++i )
            {
                int id = atoi( ids[i].cStr() );
                if( !id ) continue;
                goby::acomms::protobuf::ModemTransmission slot;
                slot.set_src( id );
                slot.set_dest( goby::acomms::QUERY_DESTINATION_ID );
                slot.set_type( goby::acomms::protobuf::ModemTransmission::DATA );
                slot.set_rate( 0 );
                slot.set_slot_seconds( 12 );
                mac_->push_back( slot );
                logger_.syslog( "Added slot ", id, Syslog::INFO );
            }
            delete[] ids;
            mac_->update();
        }
    }

    if( !isDataRequested() && !isCommanded() )
    {
        return PAUSE;
    }
    if( isCommanded() & !pinging_ )
    {
        return RUNNABLE;
    }
    if( pinging_ )
    {

    }
    GobyModemMessage message_out;
    message_out.set_source( 0 );
    if( SendData::DataToSend( "modem" ) )
    {
        SendData* data = SendData::Pop( "modem" );
        SendDestination* destination = data->getDestination();
        Str cmd;
        if( destination->getPath() != Str::EMPTY_STR )
        {
            cmd = "set " + destination->getPath() + " ";
            if( NULL != data->getUnit() )
            {
                cmd += data->getValue()->toString( *data->getUnit() ) + " " + data->getUnit()->getName();
            }
            else
            {
                cmd += "string \"" + data->getValue()->toString() + "\"";
            }
        }
        else
        {
            cmd = data->getValue()->toString();
        }
        logger_.syslog( destination->getScheme() + "://" + destination->getHost() + ": " + cmd, Syslog::INFO );
        delete data;

        if( cmd != Str::EMPTY_STR )
        {
            message_out.set_telegram( cmd.cStr(), cmd.length() );
            // send this message to my buddy!
            message_out.set_destination( destination->getHostId() );
            message_out.set_source( vehicleId_ );
            //DEBUG_LOG( "cmd is " + cmd + " from " + message_out.source() +  " to " + message_out.destination());
        }

    }

    if( !simulateHardware() )
    {
        if( message_out.source() != 0 )
        {
            q_manager_->push_message( message_out );
        }
        try
        {
            // These may call received_data_handler() or received_ack_handler()
            mm_driver_->do_work();
            mac_->do_work();
            q_manager_->do_work();
        }
        catch( std::runtime_error& e )
        {
            logger_.syslog( "exception while running: ", e.what(), Syslog::FAULT );
            this->setFailure( FailureMode::SOFTWARE );
            return STOP;
        }
    }
    else
    {
        Str messageStr( Str::EMPTY_STR );
        float delay = 2 * message_out.telegram().length() * 10 / transBaudCfg_;
        float mediumSpeed = 1500;  // Nominal speed of sound in sea water, m/s
        if( message_out.source() != 0 )
        {
            messageStr = Str( message_out.telegram().c_str(), message_out.telegram().length() );
            DEBUG_LOG( "SimSlate outgoing messageStr setting to " + messageStr );
        }
        // This can write a blank message, which prompts for a read
        SimSlate::SendMessage( message_out.destination(), message_out.source(),
                               messageStr, maxDistanceCfg_, delay, mediumSpeed );

        int destination = vehicleId_;
        int source;

        if( SimSlate::ReceiveMessage( destination, source, messageStr ) )
        {
            DEBUG_LOG( "SimSlate incoming messageStr is " + messageStr );
            GobyModemMessage message_in;
            message_in.set_destination( destination );
            message_in.set_source( source );
            message_in.set_telegram( messageStr.cStr(), messageStr.length() );
            received_data_handler( message_in );
        }
    }

    return RUNNABLE;
}


Component::RunState GobyModem::stop()
{
    DEBUG_LOG( "Stop" );

    // turn off power here
    DEBUG_LOG( "Powering down GobyModem" );
    if( !simulateHardware() )
    {
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::ERROR );
        }
    }

    uninitialize(); // First power down then query for faults next cycle

    return STOPPING;
}


Component::RunState GobyModem::stopping()
{
    DEBUG_LOG( "Stopping" );

    if( !simulateHardware() )
    {
        loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }

    }
    // Currently used to delay one more cycle to allow the EZ Servo to fully power down
    return STOPPED;
}


Component::RunState GobyModem::stopped()
{
    DEBUG_LOG( "Stopped" );
    if( isDataRequested() || isCommanded() )
    {
        return start();
    }

    if( !simulateHardware() )
    {
        if( ( loadControl_.getPowerState() != LoadControl::OFF ) && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
        {
            return stop();
        }

    }
    return STOPPED;
}

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI GobyModem::getConfigURI( ConfigOption configOption ) const
{
    switch( configOption )
    {
    case CONFIG_SIMULATE_HARDWARE:
        return GobyModemIF::SIMULATE_HARDWARE;
    default:
        return ConfigURI::NO_CONFIG_URI;
    }
    return ConfigURI::NO_CONFIG_URI;
}

bool GobyModem::isDataRequested()
{
    //DEBUG_LOG( "isDataRequested returning " + Str( platformConversationWriter_->isDataRequested() ? "true" : "false" ) );
    return platformConversationWriter_->isDataRequested();
}

bool GobyModem::isCommanded()
{
    DEBUG_LOG( "isCommanded returning " + Str( pinging_ || dusblPingCodeReader_->wasTouchedSinceLastRun( this ) ? "true" : "false" ) );
    return pinging_ || dusblPingCodeReader_->wasTouchedSinceLastRun( this );
}

void monitor_mac( const goby::acomms::protobuf::ModemTransmission& mac_msg )
{
    if( mac_msg.src() == GobyModem::Instance()->getVehicleId() )
    {
        GobyModem::Instance()->getLogger().syslog( "{control} starting send from me", Syslog::INFO );
    }
    else
    {
        GobyModem::Instance()->getLogger().syslog( "{control} sending from " + Str( mac_msg.src() ), Syslog::INFO );
    }
}

void monitor_modem_receive( const goby::acomms::protobuf::ModemTransmission& rx_msg )
{
    if( rx_msg.GetExtension( micromodem::protobuf::type ) == micromodem::protobuf::MICROMODEM_TWO_WAY_PING &&
            rx_msg.HasExtension( micromodem::protobuf::ranging_reply ) )
    {
        const micromodem::protobuf::RangingReply& range_reply = rx_msg.GetExtension( micromodem::protobuf::ranging_reply );
        if( range_reply.one_way_travel_time_size() > 0 )
        {
            //double owtt = range_reply.one_way_travel_time(0);
            GobyModem::Instance()->getLogger().syslog( range_reply.ShortDebugString().c_str(), Syslog::INFO );
        }

    }

}

void received_data_handler( const google::protobuf::Message& message_in )
{
    printf( "%s\n", message_in.GetTypeName().c_str() );
    GobyModemMessage typed_message_in;
    typed_message_in.CopyFrom( message_in );
    GobyModem::Instance()->getLogger().syslog( Str( ( int )typed_message_in.source() ) + ": " + typed_message_in.telegram().c_str(), Syslog::INFO );
    DataReceiver::Receive( typed_message_in.telegram().c_str(), typed_message_in.telegram().length(), GobyModem::Instance()->getLogger() );
}

void received_ack_handler( const goby::acomms::protobuf::ModemTransmission& ack_message,
                           const google::protobuf::Message& original_message )
{
    GobyModemMessage typed_original_message;
    typed_original_message.CopyFrom( original_message );

    GobyModem::Instance()->getLogger().syslog(
        Str( ack_message.src() ) + " { acknowledged receiving message : " + typed_original_message.telegram().c_str() + " }", Syslog::INFO );
}