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

#include "SimDaemon.h"

#include <cstdio>
#include <cstdlib>
#include <pthread.h>
#include <sys/stat.h>

#include "EnvSimulator.h"
#include "SimCommsStruct.h"
#include "SimHeadStruct.h"
#include "SimInitStruct.h"
#include "SimRunStruct.h"
#include "SimResultStruct.h"
#include "utils/Timestamp.h"

FastMap<uint64_t, EnvSimulator*> SimDaemon::SimulatorMap_;

#define EXIT_SUCCESS 0
#define EXIT_FAILURE 1

int main( int argc, char* argv[] )
{
    bool showOutputs = ( argc > 1 )
                       && ( ( strncmp( argv[1], "-c", 3 ) == 0 )
                            || ( strncmp( argv[1], "-d", 3 ) == 0 )
                            || ( strncmp( argv[1], "-dC", 4 ) == 0 ) );
    bool debug = ( argc > 1 ) && ( strncmp( argv[1], "-d", 3 ) == 0 );
    bool debugComms = debug || ( ( argc > 1 ) && ( strncmp( argv[1], "-dC", 4 ) == 0 ) );
    SimDaemon::SetDebug( debug );
    SimDaemon::SetDebugComms( debugComms );

    if( !showOutputs )
    {
        SimDaemon::Daemonize( !showOutputs );
    }

    SimDaemon simDaemon;
    simDaemon.run();
}

bool SimDaemon::Debug_( false );
bool SimDaemon::DebugComms_( false );
bool SimDaemon::Shutdown_( false );
Mutex SimDaemon::ShutdownMutex_;

SimDaemon::SimDaemon()
{}

void SimDaemon::run()
#ifndef __GAZEBO
throw( SocketException& )
#endif
{
    SocketServer * socketServer;
    try
    {
        socketServer = new SocketServer( SimDaemon::SOCKET_SERVER_PORT );
    }
    catch( SocketException & se )
    {
        fprintf( stderr, "SocketException creating server on port %d: %s\n",
                 SimDaemon::SOCKET_SERVER_PORT, se.what() );
        throw se;
    }


    while( !Shutdown_ )
    {

        SocketServer* clientSocketPtr = new SocketServer();
        try
        {
            printf( "SimDaemon waiting to accept a connection!\n" );
            socketServer->accept( *clientSocketPtr );
            printf( "SimDaemon accepted a connection!\n" );
        }
        catch( SocketException & se )
        {
            fprintf( stderr, "SocketException accepting connection on port %d: %s\n",
                     SimDaemon::SOCKET_SERVER_PORT, se.what() );
            throw se;
        }

        if( !Shutdown_ )
        {
            pthread_attr_t threadAttributes;
            pthread_attr_init( &threadAttributes );
            pthread_attr_setdetachstate( &threadAttributes, PTHREAD_CREATE_DETACHED );
            pthread_t thread;
            pthread_create( &thread, &threadAttributes, SimDaemon::ServiceClient, ( void* )clientSocketPtr );
        }
        else
        {
            delete clientSocketPtr;
        }
    }
    delete socketServer;
}

SimDaemon::~SimDaemon()
{
    MutexLocker shutdownLocker( ShutdownMutex_ );
    printf( "In SimDaemon::~SimDaemon()\n" );
    while( SimulatorMap_.size() > 0 )
    {
        EnvSimulator * simulator = SimulatorMap_.getIndexed( 0, true );
        if( NULL != simulator )
        {
            printf( "Deleting simulator at 0x%08zX\n", ( size_t )simulator );
            delete simulator;
        }
    }
}

void SimDaemon::Daemonize( bool hideOutputs )
{
    pid_t pid, sid;

    /* already a daemon */
    if( getppid() == 1 ) return;

    /* Fork off the parent process */
    pid = fork();
    if( pid < 0 )
    {
        exit( EXIT_FAILURE );
    }
    /* If we got a good PID, then we can exit the parent process. */
    if( pid > 0 )
    {
        exit( EXIT_SUCCESS );
    }

    /* At this point we are executing as the child process */

    /* Change the file mode mask */
    umask( 0 );

    /* Create a new SID for the child process */
    sid = setsid();
    if( sid < 0 )
    {
        exit( EXIT_FAILURE );
    }

    /* Change the current working directory.  This prevents the current
       directory from being locked; hence not being able to remove it. */
    //if ((chdir("/")) < 0) {
    //    exit(EXIT_FAILURE);
    //}

    /* Redirect standard files to /dev/null */
    if( NULL == freopen( "/dev/null", "r", stdin ) )
    {
        fprintf( stderr, "Error redirecting standard input to /dev/null" );
    }
    if( hideOutputs )
    {
        if( NULL == freopen( "/dev/null", "w", stdout ) )
        {
            fprintf( stderr, "Error redirecting standard output to /dev/null" );
        }
        if( NULL == freopen( "/dev/null", "w", stderr ) )
        {
            fprintf( stderr, "Error redirecting standard err to /dev/null" );
        }
    }
}

#define MAX_DATA_LENGTH ( 2048 )
void* SimDaemon::ServiceClient( void* clientSocketPtr )
{
    SocketServer& clientSocket = *( SocketServer* )clientSocketPtr;
    clientSocket.setTimeout( 30000 );

    try
    {
        EnvSimulator* simulator( NULL );
        uint64_t uuid( 0 );
        unsigned int dataLength = MAX_DATA_LENGTH;
        while( !Shutdown_ && 0 < dataLength )
        {
            char data[ MAX_DATA_LENGTH ];
            if( Debug_ ) printf( "SimDaemon waiting for data!\n" );
            dataLength = clientSocket.recv( data, MAX_DATA_LENGTH );
            if( Debug_ ) printf( "Got data of length %d:\n", dataLength );
            if( dataLength < sizeof( SimHeadStruct ) )
            {
                continue;
            }
            SimHeadStruct head;
            memcpy( ( char* )&head, data, sizeof( SimHeadStruct ) );
            if( Debug_ ) printf( "uuid=0x%16LX\n", ( long long unsigned int )head.uuid_ );
            if( Debug_ ) printf( "action=%s\n",
                                     head.structType_ == SimHeadStruct::SIM_INIT_STRUCT ? "init" :
                                     ( head.structType_ == SimHeadStruct::SIM_REINIT_STRUCT ?  "reinit" :
                                       ( head.structType_ == SimHeadStruct::SIM_RUN_STRUCT ?  "run" :
                                         ( head.structType_ == SimHeadStruct::SIM_COMMS_STRUCT ?  "comms" : "quit" ) ) ) );
            if( Debug_ ) printf( "structSize=%d\n", head.structSize_ );
            unsigned int expectedLength = head.structSize_ + sizeof( head );
            if( dataLength != expectedLength )
            {
                printf( "Wrong size of data received. Expected %d bytes, got %d\n", expectedLength, dataLength );
                dataLength = 0;
                break;
            }

            if( head.structType_ == SimHeadStruct::SIM_QUIT )
            {
                printf( "Shutting down SimDaemon\n" );
                Shutdown_ = true;
            }
            else if( head.structType_ == SimHeadStruct::SIM_INIT_STRUCT
                     || head.structType_ == SimHeadStruct::SIM_REINIT_STRUCT )
            {
                uuid = head.uuid_; // head.uuid_ is packed, so it can't be passed by reference
                simulator = SimulatorMap_.get( uuid );
                SimInitStruct initParams;
                memcpy( &initParams, data + sizeof( head ), sizeof( initParams ) );
                if( head.structType_ == SimHeadStruct::SIM_INIT_STRUCT && NULL != simulator )
                {
                    printf( "Deleting old simulator for uuid=0x%16LX\n", ( long long unsigned int )uuid );
                    delete simulator;
                    simulator = NULL;
                    SimulatorMap_.pop( uuid );
                }
                if( NULL == simulator )
                {
                    printf( "New EnvSimulator for uuid=0x%16LX\n", ( long long unsigned int )uuid );
                    simulator = new EnvSimulator( true, 10, Debug_ );
                    SimulatorMap_.put( uuid, simulator );
                }
                else if( head.structType_ == SimHeadStruct::SIM_REINIT_STRUCT )
                {
                    initParams.initLat_ = nanf( "" );
                    initParams.initLon_ = nanf( "" );
                    initParams.initP_ = nanf( "" );
                    initParams.initPitch_ = nanf( "" );
                    initParams.initQ_ = nanf( "" );
                    initParams.initR_ = nanf( "" );
                    initParams.initRoll_ = nanf( "" );
                    initParams.initU_ = nanf( "" );
                    initParams.initV_ = nanf( "" );
                    initParams.initW_ = nanf( "" );
                    initParams.initHeading_ = nanf( "" );
                    initParams.initZ_ = nanf( "" );
                }
                if( Debug_ ) printf( "initParams.initLat_=%g\n", initParams.initLat_ );
                SimResultStruct results;
                simulator->initialize( initParams, results );
                clientSocket.send( ( char* )&results, sizeof( results ) );
            }
            else if( head.structType_ == SimHeadStruct::SIM_RUN_STRUCT )
            {
                EnvSimulator * simulator = SimulatorMap_.get( uuid );
                if( NULL != simulator )
                {
                    SimRunStruct runParams;
                    SimResultStruct results;
                    memcpy( &runParams, data + sizeof( head ), sizeof( runParams ) );
                    simulator->run( runParams, results );
                    clientSocket.send( ( char* )&results, sizeof( results ) );
                }
            }
            else if( head.structType_ == SimHeadStruct::SIM_COMMS_STRUCT )
            {
                //if( DebugComms_ ) printf( "Got Comms, " );
                // Let's get the comms data
                SimCommsEntry* commsEntry = NULL;
                SimCommsStructToSim comms;
                memcpy( &comms, data + sizeof( head ), sizeof( comms ) );
                //if( DebugComms_ ) printf( "length = %d\n", comms.length_ );
                // Is there an outgoing message?
                if( comms.length_ > 0 )
                {
                    double latitude, longitude;
                    simulator->getLocation( latitude, longitude );
                    unsigned int msgDataLength = clientSocket.recv( data, MAX_DATA_LENGTH );
                    if( DebugComms_ ) printf( "Got msg data of length %d, expected %d:\n", msgDataLength, comms.length_ );
                    if( comms.length_ == msgDataLength )
                    {
                        commsEntry = new SimCommsEntry( comms, data, latitude, longitude );
                        for( unsigned int i = 0; i < SimulatorMap_.size(); ++i )
                        {
                            double simLatitude, simLongitude, simDistance;
                            EnvSimulator* sim = SimulatorMap_.getIndexed( i );
                            if( sim == simulator )
                            {
                                continue;
                            }
                            sim->getLocation( simLatitude, simLongitude );
                            simDistance = Location::GetDistanceFast( simLatitude, simLongitude, latitude, longitude );
                            if( comms.maxDistance_ > 0  && simDistance > comms.maxDistance_ )
                            {
                                continue;
                            }
                            double delay = ( comms.mediumSpeed_ > 0  ? simDistance / comms.mediumSpeed_ : 0 ) + comms.delay_;
                            commsEntry->addSim( sim, delay );
                        }
                        if( commsEntry->hasSims() )
                        {
                            AddSimCommsEntry( commsEntry );
                        }
                    }
                }
                else
                {
                    // Is there an incoming message?
                    commsEntry = FindSimCommsEntry( simulator );
                }
                SimCommsStructFromSim commsOut;
                if( commsEntry )
                {
                    commsEntry->fillCommsStructFromSim( commsOut );
                }
                //if( DebugComms_ ) printf( "Sending commsOut of length %d\n", sizeof( commsOut ) );
                clientSocket.send( ( char* )&commsOut, sizeof( commsOut ) );
                if( commsEntry && commsOut.length_ > 0 )
                {
                    if( DebugComms_ ) printf( "Sending msg data of length %d\n", commsOut.length_ );
                    clientSocket.send( commsEntry->getMessage().cStr(), commsEntry->getMessage().size() );
                }
                /*if( commsEntry && !commsEntry->hasSims() )
                {
                    delete commsEntry;
                }*/
            }
            else
            {
                clientSocket.send( NULL, 0 );
            }
        }
        printf( "Closing down connection.\n" );
        clientSocket.close();
        if( NULL != simulator )
        {
            delete simulator;
            simulator = NULL;
            SimulatorMap_.pop( uuid );
        }
    }
    catch( SocketException & se )
    {
        printf( "Got SocketException: %s\n", se.what() );
    }

    delete &clientSocket;

    return NULL;
}

/*static*/ FlexArray<SimCommsEntry*> SimDaemon::SimCommsEntries_( true );

/*static*/ void SimDaemon::AddSimCommsEntry( SimCommsEntry* entry )
{
    SimCommsEntries_.push( entry );
}

/*static*/ SimCommsEntry* SimDaemon::FindSimCommsEntry( Simulator* sim )
{
    Timestamp now( Timestamp::Now() );
    SimCommsEntry* returnEntry = NULL;
    for( unsigned int i = 0; returnEntry == NULL && i < SimCommsEntries_.size(); ++i )
    {
        SimCommsEntry* entry = SimCommsEntries_.get( i );
        //printf("Got entry at index %d: %08ZX\n", i, (size_t)entry);
        if( entry->dataForSim( sim, now ) )
        {
            returnEntry = entry;
        }
    }
    for( int i = ( int )SimCommsEntries_.size() - 1; i >= 0; --i )
    {
        SimCommsEntry* entry = SimCommsEntries_.get( i );
        if( !entry->hasSims() )
        {
            SimCommsEntries_.pop( i );
            if( entry != returnEntry )
            {
                //printf("Delete entry at index %d: %08ZX\n", i, (size_t)entry);
                delete entry;
            }
        }
    }
    return returnEntry;
}