#define _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <fcntl.h>
#include <sys/stat.h>
#include <mqueue.h>
#include <pthread.h>
#include <dlfcn.h>
#include <sched.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/syscall.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <errno.h>
#include "ucp_printf.h"
#include "ospLog.h"
#include "osp.h"


void *g_SemStartWork;
void *g_SemRegTask;

OSP_TASKCB *gOspTask;

extern OSP_UDP_TASK_DATA udp_shell_rx_task;
#ifdef HEARTBEAT_ENABLE
extern uint64_t OspTimerTask(void);
#endif
int32_t osp_task_is_shell(uint32_t taskid)
{
    return gOspTask[taskid].is_shell;
}
void osp_set_task_shell(uint32_t taskid)
{
    gOspTask[taskid].is_shell = 1;
}
int32_t osp_task_is_reged(uint32_t taskid)
{
    return gOspTask[taskid].Active;
}

int32_t osp_local_shell_task_is_reged(void)
{
    return osp_task_is_reged(localshellin);
}

int32_t osp_set_cpu(int32_t i)
{
    if (OSP_NO_AFFIINITY == i)
    {
        return 0;
    }
    else
    {
        cpu_set_t mask;
        CPU_ZERO(&mask);
        CPU_SET(i, &mask);
        osp_assert(0 == pthread_setaffinity_np(pthread_self(), sizeof(mask), &mask));
        return 0;
    }
}

OSP_TASKMSG_REG g_TaskRegTbl[] = 
{
    //{DbgLogTask,         "OspDbglog",          NOMARL_MSG_PRI(7),   NULL,       (OSP_FUNCPTR)osp_dbg_log_main,          0, OSP_PROCESS_MSG},
    //{localshellout,      "localshellout",      RT_MSG_PRI(10),      NULL,       (OSP_FUNCPTR)osp_local_shell_out_main,  0, OSP_PROCESS_MSG},
    //{localshellin,       "localshellin",       RT_NOMSG_PRI(98),    NULL,       (OSP_FUNCPTR)osp_shell_main,            0, OSP_PROCESS_MSG},
    //{remoteshellout,     "remoteshellout",     RT_NOMSG_PRI(98),    NULL,       (OSP_FUNCPTR)osp_shell_task,            0, OSP_OTHER_MSG}, 
    //{OspTickLess,        "OspTickLess",        RT_NOMSG_PRI(20),    NULL,       (OSP_FUNCPTR)osp_tick_less,             1, OSP_PROCESS_MSG},
#ifdef HEARTBEAT_ENABLE
    {OspTmrTask,         "OspTmrTask",         RT_NOMSG_PRI(99),    NULL,       (OSP_FUNCPTR)OspTimerTask,              2, OSP_OTHER_MSG},
#endif
    //{OspDbgLog,          "OspLogTask",         RT_NOMSG_PRI(75),    NULL,       (OSP_FUNCPTR)osp_oam_msg_proc_task,     0, OSP_OTHER_MSG},
    //{ospnetshellrx,      "ospnetshellrx",      NOMARL_MSG_PRI(6),   (OSP_FUNCPTR)osp_net_shell_init,    (OSP_FUNCPTR)osp_net_shell_main,        0, OSP_UDP_MSG,     (U64)&udp_shell_rx_task},

};

__thread __typeof__(uint32_t) thread_local_TaskId = 0;

const char *g_OspSoPath = {"/usr/lib/libosp.so"};
extern const char *g_UserSoPath;
uint32_t g_OspSoNum = 2;

void *osp_find_sym(char *pName)
{
    void *handle = NULL;
    void *pSym = NULL;


    {
        handle = dlopen(g_OspSoPath, RTLD_NOW);
        if (NULL == handle)
        {
            UCP_PRINT_ERROR("%s %s \r\n", g_OspSoPath, strerror(errno));
            //osp_debug_out_with_time(ERR_DEBUG_LEVEL, "%s %s \r\n", g_OspSoPath, strerror(errno));
        }
        else
        {
            pSym = dlsym(handle, pName);
            if (pSym)
                return pSym;
        }
    }

#if 0
    {
        handle = dlopen(g_UserSoPath, RTLD_NOW);
//        handle = dlopen(USER_SO_PATH, RTLD_NOW);
        if (NULL == handle)
        {
//            osp_debug_out_with_time(ERR_DEBUG_LEVEL, "%s %s \r\n", USER_SO_PATH, strerror(errno));
          osp_debug_out_with_time(ERR_DEBUG_LEVEL, "%s %s \r\n", g_UserSoPath, strerror(errno));
        }
        else
        {
            pSym = dlsym(handle, pName);
            if (pSym)
                return pSym;
        }
    }
#endif
    return pSym;
}





const uint32_t g_RegTaskNum=OSP_NELEMENTS(g_TaskRegTbl);


void osp_task_entry(OSP_TASKCB *Osp_TaskCB)
{
    struct sched_param struSchedParam;
    OSP_STATUS ret;
    pthread_t PhreadId; 
    uint32_t MaxTaskPri;
    uint32_t MinTaskPri;
    Osp_Msg_Head *pHead;
    OSP_FUNCPTR Init;

    
    CURRENT_TASKID = Osp_TaskCB->Osp_TaskMsg_Reg.TaskId;
    Osp_TaskCB->PhreadId = pthread_self();
    
    Init = Osp_TaskCB->Osp_TaskMsg_Reg.Init;
    
    if (Init)
    {
        ret = Init();
        if (ret)
        {
            osp_debug_out_with_time(ERR_DEBUG_LEVEL, "task-%s int32_t err\n", Osp_TaskCB->Osp_TaskMsg_Reg.TaskName);
            osp_suspend_task(CURRENT_TASKID);
        }
//        osp_assert(OSP_OK == ret);
    }
    Osp_TaskCB->TId = syscall(SYS_gettid);
    osp_set_cpu(Osp_TaskCB->Osp_TaskMsg_Reg.Cpu); 
    
    
    if ((NORMALTASK == ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & POLICYMASK))
        && (MSGTASK == ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & MSGMASK)))
    {
 //       sprintf(buf, "renice -n %d -p %d", ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & PRIMASK),
 //           Osp_TaskCB->TId);
 //       system(buf);
        setpriority(PRIO_PROCESS, 0, Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri);

    
        osp_sem_take(g_SemStartWork, -1);
        while (1)
        {
            pHead = osp_rev_msg();
            Osp_TaskCB->Osp_TaskMsg_Reg.MainLoop(pHead);
            TASK_RUNCNT(CURRENT_TASKID)++;
        }
    }
    else if ((NORMALTASK == ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & POLICYMASK))
        && (NOMSGTASK == ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & MSGMASK)))
    {
  //      sprintf(buf, "renice -n %d -p %d", ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & PRIMASK),
  //          Osp_TaskCB->TId);
  //      system(buf);  
    setpriority(PRIO_PROCESS, 0, Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri);

        osp_sem_take(g_SemStartWork, -1);
        while (1)
        {
            Osp_TaskCB->Osp_TaskMsg_Reg.MainLoop();
            TASK_RUNCNT(CURRENT_TASKID)++;

        }
    }
    else if (RTTASK == ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & POLICYMASK))
    {
        Osp_TaskCB->IsRt =1;
        MaxTaskPri = sched_get_priority_max(SCHED_FIFO);
        MinTaskPri = sched_get_priority_min(SCHED_FIFO);
        osp_assert((((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & PRIMASK) <= MaxTaskPri) && 
            (((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri)&PRIMASK) >= MinTaskPri));

        struSchedParam.sched_priority = ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & PRIMASK);
        
        PhreadId = pthread_self();

        
        ret = pthread_setschedparam(PhreadId, (int32_t)SCHED_FIFO, &struSchedParam);    
        osp_assert(OSP_OK == ret);
        Osp_TaskCB->OsTaskPri = getpriority(PRIO_PROCESS, Osp_TaskCB->TId);
        if (NOMSGTASK == ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & MSGMASK))
        {
            osp_sem_take(g_SemStartWork, -1);
            while (1)
            {
                Osp_TaskCB->Osp_TaskMsg_Reg.MainLoop();
                TASK_RUNCNT(CURRENT_TASKID)++;
            }
        }
        else if (MSGTASK == ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & MSGMASK))
        {
            if (XXTASK != ((Osp_TaskCB->Osp_TaskMsg_Reg.TaskPri) & XXMASK))
            {
                osp_sem_take(g_SemStartWork, -1);
                while (1)
                {
                    pHead = osp_rev_msg();

                    Osp_TaskCB->Osp_TaskMsg_Reg.MainLoop(pHead);
                    TASK_RUNCNT(CURRENT_TASKID)++;
                }
            }
            else
            {
                osp_sem_take(g_SemStartWork, -1);
                 while (1)
                {

                    osp_sem_take(Osp_TaskCB->sem, -1);
                    Osp_TaskCB->Osp_TaskMsg_Reg.MainLoop(NULL);
                    TASK_RUNCNT(CURRENT_TASKID)++;
                }               
            }
        }
    }
    else
    {
        osp_assert(0);
    }

}

static uint32_t osp_get_free_task_id(void)
{
    int32_t i;

    for (i = 1; i < OSP_MAX_TASK; i++)
    {
        if (0 == gOspTask[i].Active)
        {
            return i;
        }
    }

    return OSP_INVALID_TASKID;
}


OSP_STATUS osp_regtask(OSP_TASKMSG_REG *TaskRegTbl)
{
    pthread_t Thread;
    pthread_attr_t ThreadAttr;
    OSP_STATUS ret =0;
    uint32_t TaskId;
    
    osp_assert(NULL != TaskRegTbl);

    osp_sem_take(g_SemRegTask, -1);

    
    TaskId = TaskRegTbl->TaskId;

    if (OSP_INVALID_TASKID == TaskId)
    {
        TaskId = osp_get_free_task_id();
        if (OSP_INVALID_TASKID == TaskId)
        {
            osp_sem_give(g_SemRegTask);
            return OSP_ERROR;
        }
        TaskRegTbl->TaskId = TaskId;
    }

    osp_assert(TaskId < OSP_MAX_TASK);
    /*ospshell will retart,so this is not ok*/
    osp_assert(0 == gOspTask[TaskId].Active);
    gOspTask[TaskId].sem = osp_semc_create(0);


    if ((OSP_NO_AFFIINITY != TaskRegTbl->Cpu) && ((OSP_CPU_NUM - 1) < TaskRegTbl->Cpu))
    {
        osp_sem_give(g_SemRegTask);
        return OSP_ERROR;
    }
    
    gOspTask[TaskId].Active = 1;
    gOspTask[TaskId].State = TASK_RUNNING;
    (void)memcpy(&(gOspTask[TaskId].Osp_TaskMsg_Reg), TaskRegTbl, sizeof(OSP_TASKMSG_REG));

    if (OSP_PROCESS_MSG == TaskRegTbl->MsgType)
    {
         osp_create_msgq((Osp_MsgQ_Tcb *)(gOspMsgQue + OSP_ROUND_UP(OSP_MSG_QUE_SIZE,32) * TaskId),
            (char *)(gOspTask[TaskId].Osp_TaskMsg_Reg.TaskName));
        
        gOspTask[TaskId].send_msg_func = (OSP_FUNCPTR)osp_send_por_msg;
        gOspTask[TaskId].rev_msg_func = (OSP_FUNCPTR)osp_rev_promsg;
        gOspTask[TaskId].Msg_Que = (uint64_t)(gOspMsgQue + OSP_ROUND_UP(OSP_MSG_QUE_SIZE,32) * TaskId);
//            gOspTask[TaskId].MsgQId = QueId;
    
    }

    if (OSP_UDP_MSG == TaskRegTbl->MsgType)
    {
        int32_t rxudpid = 0;
        int32_t txudppid = 0;
        OSP_UDP_TASK_DATA *pdata;

        pdata = (OSP_UDP_TASK_DATA *)TaskRegTbl->MsgTypeData;
        
        if (pdata->IsRx)
        {
            rxudpid = osp_create_rxucp(pdata->port);
            txudppid = osp_create_txucp(pdata->port, "127.0.0.1");
        }
        else
        {
            txudppid = osp_create_txucp(pdata->port, pdata->ip);
        }
        gOspTask[TaskId].send_msg_func = (OSP_FUNCPTR)osp_send_udpmsg;
        gOspTask[TaskId].rev_msg_func = (OSP_FUNCPTR)osp_rev_udpmsg;
        gOspTask[TaskId].Msg_Que = OSP_UDPID_TO_QUE(txudppid, rxudpid);
    }

    if (OSP_OTHER_MSG == TaskRegTbl->MsgType)
    {
    
        gOspTask[TaskId].send_msg_func = (OSP_FUNCPTR)osp_send_othermsg;
        gOspTask[TaskId].rev_msg_func = (OSP_FUNCPTR)osp_rev_other_msg;
        gOspTask[TaskId].Msg_Que = TaskRegTbl->MsgTypeData;
        

    }
    /*if task has no mainloop, then it is stub task*/
    if (NULL == TaskRegTbl->MainLoop)
    {
        osp_sem_give(g_SemRegTask);
        return OSP_OK;
    }
    


    
    ret = pthread_attr_init(&ThreadAttr);
    osp_assert(OSP_OK == ret);
//        ret = pthread_attr_setstacksize(&ThreadAttr, OSP_TASK_STACK);
//        osp_assert(OSP_OK == ret); 
        
    if (IS_RT_PRI(TaskRegTbl->TaskPri))
    {
        ret = pthread_attr_setschedpolicy(&ThreadAttr, 
                                          SCHED_FIFO);        
        osp_assert(OSP_OK == ret);    
    }        
    ret = pthread_create(&Thread, 
                 &ThreadAttr, 
                 (void *(*)(void *))osp_task_entry, 
                 &gOspTask[TaskId]);
    osp_assert(OSP_OK == ret);
    
    osp_sem_give(g_SemRegTask);

    return OSP_OK;
}



OSP_STATUS osp_reg_task_tlb(OSP_TASKMSG_REG *TaskRegTbl, uint32_t TaskNum)
{
    int32_t i;

    osp_assert(NULL != TaskRegTbl);
    osp_assert(TaskNum <= OSP_MAX_TASK);
    
    for (i = 0; i < TaskNum; i++)
    {
        osp_regtask(&TaskRegTbl[i]);
    }

    return OSP_OK;
}

int32_t osp_set_net_task_msg_que(int32_t taskid, int32_t txudpid, int32_t rxudpid)
{
    if ((taskid >= OSP_MAX_TASK) || (taskid <0))
    {
        return -1;
    }
    
    gOspTask[taskid].Msg_Que = OSP_UDPID_TO_QUE(txudpid, rxudpid);

    return 0;
}
void osp_start_task_all(void)
{
    uint32_t i;
    for (i = 0; i < OSP_MAX_TASK; i++)
    {
        osp_sem_give(g_SemStartWork);
    }

}
OSP_STATUS osp_regtaskAll(void)
{
    //OSP_TASKMSG_REG *pTaskRegTbl = 0;
    //U32 *pRegTaskNum = NULL;
    OSP_STATUS ret;
    g_SemStartWork = osp_semc_create(0);
    g_SemRegTask = osp_semm_create();


    if (0 == g_ProcessId)
    {
        ret = osp_reg_task_tlb(g_TaskRegTbl, g_RegTaskNum);
        osp_assert(OSP_OK == ret);
    
    }

    usleep(100); /*to  keep setup order*/

#if 0
    pTaskRegTbl = osp_find_sym("TaskRegTbl");
    pRegTaskNum = osp_find_sym("TegTaskNum");
    if ((NULL !=pTaskRegTbl) && (NULL != pRegTaskNum))
    {
        ret = osp_reg_task_tlb(pTaskRegTbl, *pRegTaskNum);
    }
    osp_reg_task_tlb(g_TestRegTbl, g_RegTestNum);
#endif
    return OSP_OK;

}

void osp_show_task_info(void)
{
    int32_t i;
    char *pbuf;
    int32_t len = 0;
    char *p;
    char state[12] = {0};
    
    pbuf = malloc(10 * 1024);
    osp_assert(pbuf != NULL);    
        
    len = sprintf(pbuf, "%-8s%-8s%-16s%-12s%-12s%-12s%-12s%-12s%-12s%-12s%-12s%-12s%-12s%-12s\n",
        "Id", "OsId", "Name", "state", "Run", "MsgRx", "MsgTx", "Cpu-Pri", "Type", 
        "MsgAlloc", "LogMsgAlloc", "MsgFree", "MemAlloc", "MemFree");
    for (i = 0; i < OSP_MAX_TASK; i++)
    {
        if (0 == TASK_SYSID(i))
        {
            continue;
        }
        
        if (gOspTask[i].Active)
        {
            if (TASK_RUNNING == TASK_STATE(i))
                sprintf(state, "%s", "running");
            else if (TASK_SUSPEND == TASK_STATE(i))
                sprintf(state, "%s", "suspended");
            
            len += sprintf(pbuf + len, "%-8d%-8d%-16s%-12s%-12lu%-12d%-12d%d-%-12d%-12s%-12d%-12d%-12d%-12d%-12d\n", 
                gOspTask[i].Osp_TaskMsg_Reg.TaskId, TASK_SYSID(i), gOspTask[i].Osp_TaskMsg_Reg.TaskName, state, 
                TASK_RUNCNT(i), 
                TASK_MSG_RXCNT(i), TASK_MSG_TXCNT(i), 
                gOspTask[i].Osp_TaskMsg_Reg.Cpu, ((gOspTask[i].Osp_TaskMsg_Reg.TaskPri) & PRIMASK),
                gOspTask[i].IsRt ? "RT" : "*", 
                TASK_MSG_ALLOCCNT(i), TASK_LOGMSG_ALLOCCNT(i), TASK_MSG_FREECNT(i), 
                TASK_MEM_ALLOCCNT(i), TASK_MEM_FREECNT(i));
        }
    }
    for (p = pbuf; p < pbuf + len; p += (MAX_DBGINFO_LEN - 1))
    {
        printf("%s", p);
    }
    free(pbuf);
}

OSP_STATUS osp_task_init(void)
{
    osp_set_task_shell(localshellin);
    osp_set_task_shell(localshellout);
    osp_set_task_shell(ospnetshellrx);
    
    osp_regtaskAll();
    osp_task_dbg_out_enable_all();

    return OSP_OK;

}
int32_t osp_get_task_num(void)
{
    return OSP_MAX_TASK;
}
OSP_TASKCB *osp_get_taskcb_base(void)
{
    return gOspTask;
}

int32_t osp_set_taskcpu(uint8_t cpu, uint8_t pri)
{
    OSP_TASKMSG_REG task_reg_tabl[3];
    uint8_t i      = 0;
    OSP_STATUS ret = OSP_OK;

    if (cpu > 7)
    {        
        UCP_PRINT_DEBUG("coreid %d beyond 7\n", cpu);
        return OSP_ERROR;
    }

    memset(&task_reg_tabl[0], 0, sizeof(OSP_TASKMSG_REG));
    task_reg_tabl[0].TaskId = OspDbgLog;    
    strcpy((void *)&task_reg_tabl[0].TaskName,"OspDbgLog");
    task_reg_tabl[0].TaskPri  = RT_NOMSG_PRI(pri);
    task_reg_tabl[0].Init     = NULL;
    task_reg_tabl[0].MainLoop = (OSP_FUNCPTR)osp_oam_msg_proc_task;
    task_reg_tabl[0].Cpu      = cpu;
    task_reg_tabl[0].MsgType  = OSP_PROCESS_MSG;

    memset(&task_reg_tabl[1], 0, sizeof(OSP_TASKMSG_REG));
    task_reg_tabl[1].TaskId = localshellin;    
    strcpy((void *)&task_reg_tabl[1].TaskName,"localshellin");
    task_reg_tabl[1].TaskPri  = RT_NOMSG_PRI(90);
    task_reg_tabl[1].Init     = NULL;
    task_reg_tabl[1].MainLoop = (OSP_FUNCPTR)osp_shell_main;
    task_reg_tabl[1].Cpu      = cpu;
    task_reg_tabl[1].MsgType  = OSP_OTHER_MSG;

    memset(&task_reg_tabl[2], 0, sizeof(OSP_TASKMSG_REG));
    task_reg_tabl[2].TaskId = remoteshellout;
    strcpy((void *)&task_reg_tabl[2].TaskName,"remoteshellout");
    task_reg_tabl[2].TaskPri  = RT_NOMSG_PRI(91);
    task_reg_tabl[2].Init     = NULL;
    task_reg_tabl[2].MainLoop = (OSP_FUNCPTR)osp_shell_task;
    task_reg_tabl[2].Cpu      = cpu;
    task_reg_tabl[2].MsgType  = OSP_OTHER_MSG;

    for (i = 0; i < 3; i++)
    {
        ret = osp_regtask(&task_reg_tabl[i]);
    }

    return ret;
}