//--------------------------------------------------------------------------------
#include <cstdlib>
#include <iostream>
//--------------------------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/neutrino.h>
#include <sys/dispatch.h>
#define CAN_SERVER_PATH "/net/..."

void* gv_CommPtr;

class Comm
{
//****************************************************************************
//  Data structure
//****************************************************************************
public:
    typedef struct CommServer
    {
        //  How long without CAN reply for killing itself
        int timeout;
        //  Receive ID for it's COMM server role
        int comm_rcvid;
        //  Channel ID for it's CAN client role
        int can_coid;
        //  Message information
        char msg[6];
        //int msglen;
    } CommServerStruct;
//****************************************************************************
//  Thread pool lifecycle functions
//****************************************************************************
    thread_pool_attr_t 	tpi_attr;
    thread_pool_t*		tpp;
    pthread_attr_t		t_attr;

public:
	Comm(){
		// Default constructor
	    pthread_attr_init (&t_attr);

		/**************************************************/
 		gv_CommPtr = this;	// A bit tricks :-)
		/**************************************************/
		memset(&tpi_attr, 0, sizeof (tpi_attr));

		//
		// call back Functions Must be C like (static)
		//
	    tpi_attr.block_func    = WaitForMessageWrap;
	    //tpi_attr.unblock_func  = UnblockFuncWrap;
	    tpi_attr.context_alloc = ContextAllocWrap;
	    tpi_attr.handler_func  = ParseMessageWrap;
	    tpi_attr.context_free  = ContextFreeWrap;

	    tpi_attr.lo_water = 2;
	    tpi_attr.hi_water = 6;
    	tpi_attr.increment = 1;
	    tpi_attr.maximum = 10;
    	tpi_attr.attr = &t_attr;

		tpp = 0;

	    if((comm_chid_ = ChannelCreate(0)) == -1)
    	{
        	//  Could not create Channel, insert error code
        	exit(-1);
    	}
    }

    ~Comm(){
    	;
    }

private:
    int comm_chid_;         //  Channel ID
    int can_chid;

	// static members have same C signature, so they can be used as call-back

    //  Context manipulation functions when thread is created and killed
    THREAD_POOL_PARAM_T * ContextAlloc(THREAD_POOL_HANDLE_T *handle);
    static THREAD_POOL_PARAM_T* ContextAllocWrap(THREAD_POOL_HANDLE_T *handle);

    void ContextFree(THREAD_POOL_PARAM_T *param);
    static void ContextFreeWrap(THREAD_POOL_PARAM_T *param);

	//  Function to block waiting for a message from client
    THREAD_POOL_PARAM_T * WaitForMessage(THREAD_POOL_PARAM_T *ctp);
	static THREAD_POOL_PARAM_T* WaitForMessageWrap(THREAD_POOL_PARAM_T *ctp);

    //  For future versions
    void UnblockFunc(THREAD_POOL_PARAM_T *ctp);
    static void UnblockFuncWrap(THREAD_POOL_PARAM_T *ctp);

    //  Message parsing function when unblocks with received message
    int ParseMessage(THREAD_POOL_PARAM_T *ctp);
    static int ParseMessageWrap(THREAD_POOL_PARAM_T *ctp);

//****************************************************************************
//  Thread pool lifecycle functions
//****************************************************************************

//****************************************************************************
//  Thread pool lifecycle functions
//****************************************************************************

public:
	int start();
};


//****************************************************************************
//  Thread pool lifecycle functions
//****************************************************************************
//  Context manipulation functions when thread is created and killed
THREAD_POOL_PARAM_T* Comm::ContextAlloc(THREAD_POOL_HANDLE_T *handle)
{
    //  Stablish connection to CAN server
    CommServerStruct *comm_server = new CommServerStruct;
    comm_server->can_coid = open(CAN_SERVER_PATH, O_WRONLY);

    if(comm_server->can_coid == -1)
    {
        //  Failed to connect to CAN server
        //  Return error
		exit(-1);
    }
    //  Initialize values;
    comm_server->comm_rcvid = -1;
    return ((THREAD_POOL_PARAM_T*)comm_server);
}
THREAD_POOL_PARAM_T* Comm::ContextAllocWrap(THREAD_POOL_HANDLE_T *handle)
{
	Comm* pComm=(Comm*)gv_CommPtr;
	return( pComm->ContextAlloc(handle));
}

void Comm::ContextFree(THREAD_POOL_PARAM_T *param)
{
    CommServerStruct *comm_server = (CommServerStruct *)param;
    delete(comm_server);
}
void Comm::ContextFreeWrap(THREAD_POOL_PARAM_T *param)
{
	Comm* pComm=(Comm*)gv_CommPtr;
	return( pComm->ContextFree(param));
}

//  Function to block waiting for a message from client

THREAD_POOL_PARAM_T* Comm::WaitForMessage(THREAD_POOL_PARAM_T *ctp)
{
    //  Cast pointer
    CommServerStruct *comm_server = (CommServerStruct *)ctp;
    //  Blocks waiting for message
    MsgReceive(comm_chid_, comm_server->msg, sizeof(comm_server->msg), NULL);
    return ((THREAD_POOL_PARAM_T*)comm_server);
}
THREAD_POOL_PARAM_T* Comm::WaitForMessageWrap(THREAD_POOL_PARAM_T *ctp)
{
	Comm* pComm=(Comm*)gv_CommPtr;
	return( pComm->WaitForMessage(ctp));
}

//  Message parsing function when unblocks with received message
int Comm::ParseMessage(THREAD_POOL_PARAM_T *ctp)
{
    //  Cast pointer
    CommServerStruct *comm_server = (CommServerStruct *)ctp;
    char return_message[16];
    //  Pass the message over to CAN server
    MsgSend(comm_server->can_coid, comm_server->msg, sizeof(comm_server->msg), return_message, sizeof(return_message));
    return 1;
}
int Comm::ParseMessageWrap(THREAD_POOL_PARAM_T *ctp)
{
	Comm* pComm=(Comm*)gv_CommPtr;
	return( pComm->ParseMessageWrap(ctp));
}

//****************************************************************************
//  Communications set up and start
//****************************************************************************
int Comm::start()
{
	if(0 != tpp)
		return(1);

    //  Create a thread pool that returns
    if(0 == (tpp = thread_pool_create (&tpi_attr, 0)))
    {
        //  Could not create thread pool
        return(-1);
    }

    if (0 > thread_pool_start(tpp) )
    {
        //  Could not start thread pool
        ChannelDestroy(comm_chid_);
		comm_chid_=0;
        //sleep (3000);
        return -1;
    }
    //  If it gets here, thread pool is up and running
    return 1;
}


int main(int argc, char *argv[]) {
	void* pObj;
	Comm comm;

	pObj=(void*)&comm;     // If obj ref should be passed esxplicitly...

	///
	comm.tpi_attr.block_func(0); // Just test call-back
	comm.start();

	return EXIT_SUCCESS;
}