
#include <stdio.h>
#include <string.h>
#include <ctype.h> 
#include <time.h>
#include <time.h>
#include <sys/time.h>

/* MicroC/OS-II definitions */
#include "includes.h"

/* include common_defines.h */
#include "common_defines.h"

/* Unimation Server definitions */
#include "unimation_client.h"
#include "unimation_server.h"
#include "puma_states.h"
#include "alt_error_handler.h"

/* Include debug log  header*/
#include "log_api.h"

/* Nichestack definitions */
#include "ipport.h"
#include "tcpport.h"

/* local functin definitions*/
static INT32S WriteIPCMsgHeader(INT8U pad, INT16U u16Length, INT8U u8Cmd,
                             INT8U *pData);
                             
static INT32S translateToIPCMsge(INT16U u16DataLen, INT8U u8Cmd,
                        INT8U *data, INT8U *ipcMsg, INT32U maxSize);
                        
/*
 * This UnimationControllerQ MicroC/OS-II message queue will be used to communicate
 * between the Unimation client and Nios Development Board LED control
 * tasks.
 *
 * Handle to our MicroC/OS-II Command Queue and variable definitions related to
 * the Q for sending commands received on the Unimation controller from the
 * UnimationSlaveTask to the UnimationServerTask.
 */
OS_EVENT  *PumaStatesRecvQ;
void *PumaStatesRecvQTbl[UNIMATION_MSG_Q_SIZE]; /*Storage for UnimationCommandQ*/
struct MESSAGE_Q  stateRecvQ[UNIMATION_MSG_Q_SIZE];


/* Definition of Task Stacks for tasks not invoked by TK_NEWTASK 
 * (do not use NicheStack) 
 */
OS_STK    PumaStatesTaskStk[TASK_STACKSIZE];

/*
 * Create our MicroC/OS-II resources. All of the resources beginning with 
 * "SSS" are declared in this file, and created in this function.
 */
void PumaCreateOSDataStructs(void)
{
    int i=0;
    /*
    * Create the resource for our MicroC/OS-II Queue for sending commands
    * received on the TCP/IP socket from the UnimationServerTask()
    * to the UnimationClientTask().
    */
    fprintf(stdout, "Call PumaCreateOSDataStructs!\r\n");
   
    /*
    * Create the resource for our MicroC/OS-II Queue for sending commands
    * received on the Unimation controller from the UnimationSlaveTask()
    * to the UnimationServerTask().
   */
    PumaStatesRecvQ = OSQCreate(&PumaStatesRecvQTbl[0], UNIMATION_MSG_Q_SIZE);
    if (!PumaStatesRecvQ)
    {
        alt_uCOSIIErrorHandler(EXPANDED_DIAGNOSIS_CODE,
                "Failed to create PumaStatesRecvQ.\r\n");
    }
    for (i=0; i < UNIMATION_MSG_Q_SIZE; i++)
    {
    	PumaStatesRecvQTbl[i] = (void*) &stateRecvQ[i];
    }

}

/* This function creates tasks used in this example which do not use sockets.
 * Tasks which use Interniche sockets must be created with TK_NEWTASK.
 */
void PumaStateCreateTasks(int fd)
{
    INT8U error_code;
    fprintf(stdout, "Create PumaStatesTask!\r\n");

    error_code = OSTaskCreateExt(PumaStatesTask,
                             &fd,
                             (void *)&PumaStatesTaskStk[TASK_STACKSIZE-1],
                             PUMA_STATE_TASK_PRIORITY,
                             PUMA_STATE_TASK_PRIORITY,
                             PumaStatesTaskStk,
                             TASK_STACKSIZE,
                             NULL,
                             0);
   
    alt_uCOSIIErrorHandler(error_code, 0);
      
}



/*
 * PumaStatesTask()
 * 
 * This MicroC/OS-II thread spins forever after first establishing a listening
 * socket for the connection, binding it, and listening. Once setup,
 * it perpetually waits for incoming data to either the listening socket, or
 * (if a connection is active), the sss data socket. When data arrives, 
 * the approporiate routine is called to either accept/reject a connection
 * request, or process incoming data.
 */
void PumaStatesTask()
{
    INT32S socketfd;
    struct sockaddr_in server;

    INT8U error_code;
    INT32S retval;
    INT32S sendFrmLen;
    INT32S serverConnected = 0;
    struct MESSAGE_Q  *msg = NULL;
        
    INT8U sendData[MAX_IPC_MESSAGE_SIZE];
    /*
     * Sockets primer...
     * The socket() call creates an endpoint for TCP of UDP communication. It
     * returns a descriptor (similar to a file descriptor) that we call fd_listen,
     * or, "the socket we're listening on for connection requests" in our sss
     * server example.
     *
     * Traditionally, in the Sockets API, PF_INET and AF_INET is used for the
     * protocol and address families respectively. However, there is usually only
     * 1 address per protocol family. Thus PF_INET and AF_INET can be interchanged.
     * In the case of NicheStack, only the use of AF_INET is supported.
     * PF_INET is not supported in NicheStack.
     */
    if ((socketfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
    {
      printf_error("[UnimationServerTask] Socket creation failed!: %d", EXPANDED_DIAGNOSIS_CODE);
    }

    /* Initalize server address struct ( hardcode here)
    */
    server.sin_family = AF_INET;
    server.sin_port = htons(PUMA_STATE_PORT);
    server.sin_addr.s_addr = (SERVER_IP0 << 24) | (SERVER_IP1 << 16) | (SERVER_IP2 << 8) | SERVER_IP3;
    
    /* connect to the server */
    retval = connect(socketfd,(struct sockaddr *)&server,sizeof(server));
    if (retval < 0)
    {
    	printf_error("[PumaStatesTask : fail to connect %d.%d.%d.%d:%d\n", 
           SERVER_IP0, SERVER_IP1, SERVER_IP2, SERVER_IP3,PUMA_STATE_PORT );
    	close(socketfd);
        OSTimeDlyHMSM(0,0,5,0); //wait for 5 seconds
    }
    else
    {
    	fprintf(stdout,"[%s] PumaStateTask: report puma status to %d.%d.%d.%d:%d\n",__FUNCTION__,
                        SERVER_IP0, SERVER_IP1, SERVER_IP2, SERVER_IP3,PUMA_STATE_PORT );
    	serverConnected = 1;
    }
    while(1)
    {
        //Send to net client: forward message from the device first
    	if ( !serverConnected)
    	{
    	   //connect to the server
    		retval = connect(socketfd,(struct sockaddr *)&server,sizeof(server));
    		if ( retval < 0)
    		{
    			OSTimeDlyHMSM(0,0,5,0); //retry after 5 seconds
    			close(socketfd);
    			continue;
    		}
    		else
    		{
    			serverConnected = 1;
    		}

    	}
        msg = (struct MESSAGE_Q *)OSQAccept(PumaStatesRecvQ, &error_code);
        while (msg)
        {
                       
            sendFrmLen = translateToIPCMsge(msg->u16Length, msg->cmd, msg->data, sendData, MAX_IPC_MESSAGE_SIZE);
            fprintf(stdout,"[%s]: send IPC message: 0x%02x, length=%ld\r\n",
                            __FUNCTION__, msg->cmd, sendFrmLen);
            if (sendFrmLen > 0)
            {
                retval = send (socketfd,  // SOCKET s
                       (char*) sendData,   // const char* buf
                        sendFrmLen,               // int len
                        0); 
                if (retval < 0)
                {
                    printf_error("Failed to send %d length frame : status=%d\n", sendFrmLen, retval);
                    serverConnected = 0;
                    close(socketfd);
                    break;                
                }
            }

            msg = (struct MESSAGE_Q *)OSQAccept(PumaStatesRecvQ, &error_code);
        }
        
        /* Wait 100 milliseconds between pattern updates, to make the pattern slow
         * enough for the human eye, and more importantly, to give up control so
         * MicroC/OS-II can schedule other lower priority tasks. */
        OSTimeDlyHMSM(0,0,0,100);
        
    } /* while(1) */
    
    if (socketfd )
    {
        close(socketfd);
    }
      
    fprintf(stdout,"[%s] quit!\n",__FUNCTION__ );
}

/* WriteIPCMsgHeader, 
 *               Preappend IPCMessage header , append terminators.
 */
INT32S WriteIPCMsgHeader(INT8U pad, INT16U u16Length, INT8U u8Cmd,
                             INT8U *pData)
{
    struct timeval tmVal;
    static INT32U  u32IpcSeq;
    static INT8U   u8StateSeq;
    unsigned char i=0;

    IpcMessageHeader * ipcHdr = (IpcMessageHeader *) pData;

    ipcHdr->sync = MSG_START_SYNC;

    gettimeofday( &tmVal, NULL );
    ipcHdr->time_sec = tmVal.tv_sec;
    ipcHdr->time_nsec = tmVal.tv_usec * 1000 ; //nano seconds
    ipcHdr->sn = (u32IpcSeq++);
    ipcHdr->src = UNIMATION_MODULE_ID;
    ipcHdr->dst = UNIMATION_HOST_ID;
    ipcHdr->type = u8Cmd;
    ipcHdr->size = sizeof(IpcMessageHeader) +  sizeof(UNIMATION_CMD_HEADER) + u16Length  + 2 + pad;
    
     INT8U *ptr = (INT8U *) (pData + sizeof(IpcMessageHeader));
    //HDLC frame Header DWord
    *(ptr++) = CMD_FRM_START; /* Binary frame*/
    *(ptr++) = PROTO_TYPE & 0xFF;   /* protocol version */
    *(ptr++) = (u8StateSeq++);
    *(ptr++) = pad;
    // Command header DWORD
    *(ptr++) = u16Length & 0xFF; /* HDLC payload length : not include 2 bytes header*/
    *(ptr++) = (u16Length  >> 8) & 0xFF;
    *(ptr++) = u8Cmd ; /* little endian */
    *(ptr++) = 0x00;
    //Variable Command payload
    ptr += u16Length;
    
    for (i=0; i < pad; i++)
    {
		*(ptr++) = 0xaa;
    }
      
    // two characters terminators
    *(ptr++) = CMD_FRM_CR_END;
    *(ptr++) = CMD_FRM_LF_END;
    //return total frame length
    return (ptr - pData);
}

/* Function name: translateToIPCMsge
 *  Enqueue messages to be sent to the host */
static INT32S translateToIPCMsge(INT16U u16DataLen, INT8U u8Cmd,
                        INT8U *data, INT8U *ipcMsg, INT32U maxSize)
{

    int s32FrameLen = 0;
    INT8U          u8Pad =  0;

    if (((u16DataLen + 2) & 0x03 )> 0)
    {
        u8Pad = 4 - ((u16DataLen + 2) & 0x03 );
    }
     
    s32FrameLen = u16DataLen + sizeof(IpcMessageHeader) +  sizeof(UNIMATION_CMD_HEADER) + 2 + u8Pad ;

    if ( s32FrameLen > maxSize)
    {
        fprintf(stderr, "[%s]: data size :%d too large to send\r\n",__FUNCTION__, u16DataLen);
        return 0;
    }

    /* Compose Frame header */
    memset(ipcMsg, 0, s32FrameLen);
    WriteIPCMsgHeader(u8Pad, u16DataLen,u8Cmd, ipcMsg);
    /* Compose payload */
    if ( data && u16DataLen > 0)
    {
        memcpy(ipcMsg + sizeof(IpcMessageHeader) +  sizeof(UNIMATION_CMD_HEADER) , data, u16DataLen);
    }

    DbgHexdump(LOGAPP_DEBUG,"Enqueue TX message ",ipcMsg,  s32FrameLen);

    return s32FrameLen;
}

