#include "event.h"

#include "GeminiCommsPublic.h"
#include "GeminiStructuresPublic.h"

#include <boost/thread.hpp>
#include <boost/asio.hpp>

#include <iostream>
#include <string>
#include <csignal>

using namespace std;

static unsigned int uiSonarAltIPAddress= 0;
static bool fGotStatusPacket = false;
static int iHeadType = -1;
static unsigned short usSonarID = 0;
static bool fRun = true;
static Event hEvent;
static Event hExitEvent;

static void CallBackFn(int eType, int len, char *dataBlock)
{
    CGemStatusPacket *pStat;
    CGemStatusPacket *copyStatus;
    CGemBearingData  *pBearing;
    CGemBearingData  *copyBearing;

    switch (eType)
    {
    case PING_HEAD:
        cout << "PING_HEAD" << endl;
        break;
    case PING_DATA:
        //cout << "PING_DATA" << endl;
        break;
    case PING_TAIL:
        cout << "PING_TAIL" << endl;
        //ReleaseMutex( hEvent );
        hEvent.Notify();
        break;
    case PING_TAIL_EX:
        cout << "PING_TAIL_EX" << endl;
        //ReleaseMutex( hEvent );
        hEvent.Notify();
        break;

    case GEM_STATUS:
    {
        // Copy the data, so that we do something with it
        pStat = (CGemStatusPacket *)dataBlock;

        //In 720is/720ik there are two types of status messages received from.
        // 1. Beamformer
        // 2. Data Acqusition
        if ( pStat->m_head.m_src_sub_device_id == 0 )
        {
        }
        else if ( pStat->m_head.m_src_sub_device_id == 2 )//BEAMFORMER_FPGA status message
        {
        }
        else
        {
            break; //DA message contains empty status message and we don't need to fill following detail
        }

        fGotStatusPacket = true;


        copyStatus = new CGemStatusPacket;

        *copyStatus = *pStat;

        iHeadType = ( copyStatus->m_flags >> 8 );
        uiSonarAltIPAddress = copyStatus->m_sonarAltIp;
        usSonarID = copyStatus->m_sonarId;

        // Set to use alternate IP address of the sonar
        GEMX_UseAltSonarIPAddress(usSonarID,
                     ( (uiSonarAltIPAddress >> 24) & 0xFF ),
                     ( (uiSonarAltIPAddress >> 16) & 0xFF ),
                     ( (uiSonarAltIPAddress >>  8) & 0xFF ),
                     ( (uiSonarAltIPAddress >>  0) & 0xFF ),
                     255, 255, 255, 0);


        boost::asio::ip::address_v4 fixedSonarIp( pStat->m_sonarFixIp );
        boost::asio::ip::address_v4 altSonarIp( pStat->m_sonarAltIp );
        boost::asio::ip::address_v4 subMask( pStat->m_subnetMask );
        boost::asio::ip::address_v4 surfaceIP( pStat->m_surfaceIp );

        // Do something with the message, remember to delete copyStatus
        cout << "Sonar ID from Status Packet: " << std::dec << copyStatus->m_sonarId << endl;
        cout << "Mac Address : " << std::hex <<
            ( copyStatus->m_macAddress3 >> 8 ) << ":" << ( copyStatus->m_macAddress3 & 0xFF ) << ":" <<
            ( copyStatus->m_macAddress2 >> 8 ) << ":" << ( copyStatus->m_macAddress2 & 0xFF ) << ":" <<
            ( copyStatus->m_macAddress1 >> 8 ) << ":" << ( copyStatus->m_macAddress1 & 0xFF ) <<
            endl;
        cout << "Fixed IP: " << fixedSonarIp.to_string( ) << endl;
        cout << "Sonar alternate IP : " << altSonarIp.to_string( ) << endl;
        cout << "Surface IP: " << surfaceIP.to_string( ) << endl;
        cout << "Subnet Mask : " << subMask.to_string() << endl;
        cout << "VDSL Downstream Speed : " << std::dec << copyStatus->m_VDSLDownstreamSpeed1 << endl;
        cout << "VDSL Upstream Speed : " << std::dec << copyStatus->m_VDSLUpstreamSpeed1 << endl;
        cout << "HeadType: " << iHeadType << endl;
        cout << endl;

        // free up allocated memory
        delete copyStatus;
        break;
    }
    case GEM_ACKNOWLEDGE:
        cout << "GEM_ACKNOWLEDGE" << endl;
        break;
    case GEM_BEARING_DATA:
    {
        cout << "GEM_BEARING_DATA" << endl;

        // Copy the data, so that we can post a message
        pBearing = (CGemBearingData *)dataBlock;

        copyBearing = new CGemBearingData;


        *copyBearing = *pBearing;              // The bearing data structure contains a pointer to a block of memory which is
                                               // allocated in the DLL and which is filled with the data we want to display.
                                               // Therefore we cannot just take a copy of the structure, as this contains the
                                               // pointer to the memory allocated by the DLL.

        copyBearing->m_pData = (unsigned char *)malloc(copyBearing->m_noSamples); // Attempt to allocate the memory for the copied data

        // Did we allocate the memory?
        if (copyBearing->m_pData)
        {
            // Copy the data from the original structure to the new structure
            //memcpy(copyBearing->m_pData, pBearing->m_pData, copyBearing->m_noSamples);
        }
        else
        {
            // Failed to allocate memory? Indicate by setting number of samples to zero
            copyBearing->m_noSamples = 0;
        }
        // free up the memory we allocated
        free(copyBearing->m_pData);
        delete copyBearing;

        break;
    }
    default:
        ;//cout << eType << endl << endl;
    }
}

// Send Ping messages to the sonar
static void SendPing( unsigned short deviceID )
{
    GEMX_SetPingToDefaults( deviceID );
    GEMX_SetExtModeOutOfWaterOverride( deviceID, 1 );
    GEMX_AutoPingConfig( deviceID, 50, 60,1500);

    GEMX_SendGeminiPingConfig( deviceID );
}

static void PingThread( )
{
    while( fRun )
    {
        hEvent.WaitForEvent( 1000 );
        {
            SendPing( usSonarID );
        }
    }
    GEMX_DeleteSonarID( usSonarID );
    hExitEvent.Notify();
}


int main()
{
    boost::thread* localThread;

    int versionLength = GEM_GetVNumStringLen();
    char *versionStr = new char[ versionLength + 1 ];

    GEM_GetVNumString( versionStr, versionLength );

    cout << versionStr << endl;

    delete versionStr;

    if ( !GEM_StartGeminiNetworkWithResult( 0 ) )
    {
        cout << "GEM_StartGeminiNetworkWithResult(0) failed!" << endl;
        return -1;
    }
    else
    {
        cout << "Gemini network configured successfully !!!" << endl;
    }

    /*
    * Software Mode
    */
    GEM_SetGeminiSoftwareMode( (char*)"EvoC" );

    /*
    * Set the callback function for gemini sdk data
    */
    GEM_SetHandlerFunction( CallBackFn );

    //Wait until get a status message
    while (!fGotStatusPacket && iHeadType == -1)
    {
        cout << "Waiting for status packet...." << endl;
        boost::this_thread::sleep(boost::posix_time::milliseconds( 1000 ));
    }

    cout << "Gem sonar ID from function: " << usSonarID << endl;

    // Set to use alternate IP address of the sonar
    GEMX_UseAltSonarIPAddress(usSonarID,
                 ( (uiSonarAltIPAddress >> 24) & 0xFF ),
                 ( (uiSonarAltIPAddress >> 16) & 0xFF ),
                 ( (uiSonarAltIPAddress >>  8) & 0xFF ),
                 ( (uiSonarAltIPAddress >>  0) & 0xFF ),
                 255, 255, 255, 0);


    // Set Head type
    GEMX_SetHeadType( usSonarID, iHeadType );

    // Use alternate IP
    GEMX_TxToAltIPAddress(usSonarID, 1);

    // Set ping to defaults
    GEMX_SetPingToDefaults( usSonarID );

    // Keep sonar active
    GEMX_SetExtModeOutOfWaterOverride( usSonarID, 1 );

    //Start thread for pinging
    localThread = new boost::thread(&PingThread );

    char a = 'a';
    do
    {
        cin >> a;
    } while (a != 'z');

    fRun = false;
    hEvent.Notify();

    hExitEvent.WaitForEvent( -1 ); // For Infinite, until thread is exited
    GEM_SetHandlerFunction(NULL);
    GEMX_DeleteSonarID( 0 );
    GEM_StopGeminiNetwork();

    localThread->interrupt();
    localThread->join(); // make sure that the internal thread is gone

    return 0;
}