/*
 *Copyright (c) 2024 Black Sesame Technologies
 *
 *Licensed under the Apache License, Version 2.0 (the "License");
 *you may not use this file except in compliance with the License.
 *You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 *Unless required by applicable law or agreed to in writing, software
 *distributed under the License is distributed on an "AS IS" BASIS,
 *WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *See the License for the specific language governing permissions and
 *limitations under the License.
 */

#include "gtc_nl_msg.h"
#include "bstipc_cfg.h"

__s64 n_pps_sec;
__u32 n_pps_nsec;

namespace hanhai
{

    GTCNLMsg::GTCNLMsg()
	{

	}

    GTCNLMsg::~GTCNLMsg()
    {

    }

    bool GTCNLMsg::InitPtpDevice()
    {
        // 打开PTP设备
        m_ptp_fd = open("/dev/ptp0", O_RDWR);
        if (m_ptp_fd < 0) {
             printf("failed to open /dev/ptp0 \n") ;
            return false;
        }

        // 获取时钟ID
        m_clock_id = FROM_FD_TO_CLOCKID(m_ptp_fd);

        // 初始化PTP设备
        struct ptp_extts_request extts_request;
        memset(&extts_request, 0, sizeof(extts_request));
        extts_request.flags = 1;
        extts_request.index = 3;

        if (ioctl(m_ptp_fd, PTP_EXTTS_REQUEST, &extts_request)) {
            printf("failed to request extern timestamp");
            close(m_ptp_fd);
            return false;
        }

        return true;
    }



    bool GTCNLMsg::PpsOutEnableSet()
    {
        struct timespec ts;
        if (clock_gettime(m_clock_id, &ts)) {
            printf("failed to get clock time[/dev/ptp0] \n");
            close(m_ptp_fd);
            return false;
        }
        struct ptp_perout_request perout_request;
        memset(&perout_request, 0, sizeof(perout_request));
        perout_request.index = 3;
        perout_request.period.sec = 1;
        perout_request.period.nsec = 0;
        perout_request.flags = 0;
        perout_request.start.sec = ts.tv_sec + 2;
        perout_request.start.nsec = 0;
        n_pps_sec = perout_request.start.sec;
        n_pps_nsec = perout_request.start.nsec;
        if (ioctl(m_ptp_fd, PTP_PEROUT_REQUEST2, &perout_request)) {
            printf("failed to set ioctl PTP_PEROUT_REQUEST2 \n");
            close(m_ptp_fd);
            return false;
        }
        return true;
    }


    int GTCNLMsg::GenlSendMsg(int sock_fd, int16_t family_id, int32_t nlmsg_pid,
            int8_t genl_cmd, int8_t genl_version, int16_t nla_type,
            void *nla_data, int nla_len)
    {
        char *buf;
        gtc_msg_t msg;
        struct nlattr *na;
        int ret = -1, buflen;
        struct sockaddr_nl dst_addr;

        /* family id 0 is reserve for ctrl */
        if (family_id == 0) {
            printf("%s family id is invalid\n", __func__);
            return -1;
        }

        /* construct netlink header */
        msg.nlh.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
        msg.nlh.nlmsg_type = family_id;
        msg.nlh.nlmsg_flags = NLM_F_REQUEST;    //request msg
        msg.nlh.nlmsg_seq = 0;
        msg.nlh.nlmsg_pid = nlmsg_pid;

        /* construct genl netlink header */
        msg.gnlh.cmd = genl_cmd;
        msg.gnlh.version = genl_version;
        na = (struct nlattr *)GENLMSG_DATA(&msg);
        na->nla_type = nla_type;
        na->nla_len = nla_len + 1 + NLA_HDRLEN;
        memcpy(NLA_DATA(na), nla_data, nla_len);
        msg.nlh.nlmsg_len += NLMSG_ALIGN(na->nla_len);
        buf = (char *) &msg;
        buflen = msg.nlh.nlmsg_len;

        /* construct dest addr */
        memset(&dst_addr, 0, sizeof(dst_addr));
        dst_addr.nl_family = AF_NETLINK;
        dst_addr.nl_pid = 0;    //dest pid is 0
        dst_addr.nl_groups = 0; //unicast

        /* send msg to kernel */
        while ((ret = sendto(sock_fd, buf, buflen, 0, (struct sockaddr *)&dst_addr,
                sizeof(dst_addr))) < buflen) {
            if (ret > 0) {
                buf += ret;
                buflen -= ret;
            } else if (errno != EAGAIN) {
                return -1;
            }
        }

        return 0;
    }

    /**
    * get gtc family id
    *
    * @sock_fd: client socket
    * @family_name: family name registered on kernel
    *
    * return:
    * 0: success; -1: fail
    */
    int GTCNLMsg::GetGtcFamilyId(int sock_fd, char *family_name, __u32 *event_group)
    {
        gtc_msg_t ans;
        struct nlattr *na;
        struct nlattr *grps;
        struct nlattr *grp;
        int id, ret, rep_len, len;

        ret = GenlSendMsg(sock_fd, GENL_ID_CTRL, 0, CTRL_CMD_GETFAMILY, 1,
                        CTRL_ATTR_FAMILY_NAME, (void *)family_name,
                        strlen(family_name) + 1);
        if (ret) {
            printf("%s send genl msg fail\n", __func__);
            return -1;
        }

        rep_len = recv(sock_fd, &ans, sizeof(ans), 0);
        if (rep_len < 0) {
            printf("%s get family id fail\n", __func__);
            return -1;
        }

        if (ans.nlh.nlmsg_type == NLMSG_ERROR || !NLMSG_OK((&ans.nlh), rep_len))
        {
            printf("%s get kernel msg is invalud\n", __func__);
            return -1;
        }

        na = (struct nlattr *)GENLMSG_DATA(&ans);
        //na = (struct nlattr *)((char *)na + NLA_ALIGN(na->nla_len));

        len = 0;
        rep_len = GENLMSG_PAYLOAD(&ans.nlh);
        na = (struct nlattr *)GENLMSG_DATA(&ans);
        while (len < rep_len) {
            len += NLA_ALIGN(na->nla_len);
            if (na->nla_type == CTRL_ATTR_FAMILY_ID) {
                id = *(__u16 *)NLA_DATA(na);
            } else if (na->nla_type == CTRL_ATTR_MCAST_GROUPS) {
                struct nlattr *nested_na;
                struct nlattr *group_na;
                int group_attr_len;
                int group_attr;

                nested_na = (struct nlattr *)((char *)na + NLA_HDRLEN);
                group_na = (struct nlattr *)((char *)nested_na + NLA_HDRLEN);
                group_attr_len = 0;

                for (group_attr = CTRL_ATTR_MCAST_GRP_UNSPEC;
                    group_attr < CTRL_ATTR_MCAST_GRP_MAX; group_attr++) {
                    if (group_na->nla_type == CTRL_ATTR_MCAST_GRP_ID) {
                        *event_group = *(__u32 *)((char *)group_na +
                                    NLA_HDRLEN);
                        break;
                    }

                    group_attr_len += NLA_ALIGN(group_na->nla_len) +
                            NLA_HDRLEN;
                    if (group_attr_len >= nested_na->nla_len)
                        break;

                    group_na = (struct nlattr *)((char *)group_na +
                                    NLA_ALIGN(group_na->nla_len));
                }
            }
            na = (struct nlattr *)((char *)GENLMSG_DATA(&ans) + len);
        }

        return id;
    }

    bool  GTCNLMsg::TodppsMasterEnableSet(bool masterEnable)
    {
        printf("TodppsMasterEnableSet %d",masterEnable);
        if(masterEnable)
        {
            uint8_t pid = CPU_5;
            uint8_t fid = DEF;
            uint8_t sid = 8U;
            timesync_msgbox_client_init(CPU_5,DEF,8U);
            bool ptp_res = InitPtpDevice();
            bool pps_res = PpsOutEnableSet();
            printf("ptp_res %d pps_res: %d",ptp_res,pps_res);
            if(ptp_res )
            {
                printf("master_flag %d", master_flag);
                master_flag = true;
                return true;
            }
            return false;
        }
        else
        {
            master_flag = false;
        }
        return false;
    }

    int GTCNLMsg::InitSocket()
    {
        user_msg_t umsg;
        struct sockaddr_nl src_addr;
        int ret;
        __u32 group = 0xf;

        /* create a socket */
        sock_fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC);
        if (sock_fd < 0) {
            printf("gtc user socket creation fail: %s\n", strerror(errno));
            return -1;
        }

        family_id = GetGtcFamilyId(sock_fd, GTC_GENL_NAME, &group);
        if (family_id == 0) {
            printf("get gtc family id fail\n");
            return -3;
        } else {
            printf("gtc family id is %d group id is %d\n", family_id, group);
        }

        /* prepare bind parms */
        memset(&src_addr, 0, sizeof(src_addr));
        src_addr.nl_family = AF_NETLINK;
        src_addr.nl_pid = getpid();
        //src_addr.nl_pid = 12345;
        src_addr.nl_groups = group;

        /* action bind */
        ret = bind(sock_fd, (struct sockaddr *)&src_addr, sizeof(src_addr));
        if (ret < 0) {
            printf("gtc user socket bind fail: %s\n", strerror(errno));
            close(sock_fd);
            return -2;
        }

        if (setsockopt(sock_fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,
		       &group, sizeof(group)) < 0)
		    printf("could not join the gtc mcast group\n");

        /* send pid to gtc */
        umsg.flag = 1;      //1: pid flag, 0: normal data
        umsg.data = src_addr.nl_pid;

        ret = GenlSendMsg(sock_fd, family_id, src_addr.nl_pid, GTC_CMD_USER_INFO, 1, GTC_ATTR_USER_INFO, &umsg, sizeof(umsg));
        if (ret) {
            printf("%s send genl msg fail, ret = %d\n", __func__, ret);
            return -4;
        } else {
            printf("%s send user pid %d success\n", __func__, umsg.data);
        }

        //  int flags = fcntl(sock_fd, F_GETFL, 0);
        // if (flags < 0) {
        //     printf("fcntl failed to get flags \n");
        //     close(sock_fd);
        //     return -1;
        // }
        // flags |= O_NONBLOCK;
        // if (fcntl(sock_fd, F_SETFL, flags) < 0) {
        //     printf("fcntl failed to set flags \n");
        //     close(sock_fd);
        //     return -1;
        // }

        /* set to non-block mode */
        fcntl(sock_fd, F_SETFL, O_NONBLOCK);
        pfd.fd = sock_fd;
        pfd.events = POLLIN; // listen readable events
        pfd.revents = 0;

        return 0;
    }

    int GTCNLMsg::GetGTCMsgInfo()
    {
        struct nlmsgerr *err;
        gtc_msg_t gtc_msg;
        time_sync_parm_t *sync_info;
        struct nlattr *nla;
        int ret, len;
        uint64_t curr_gtc_count{0};
        int time_out_count = 0;
        bool socket_init_status = false;

        ret = InitSocket();
        if(ret != 0)
        {
            printf("socket init error");
            return -1;
        }
        socket_init_status = true;

        while (!stop_flag) {
            /* use poll to wait for events without occupy the CPU,
            *  -1 means infinite waiting 
            * set time out 10s
            */

            ret = poll(&pfd, 1, 10*1000); 
            if (ret < 0) {
                printf("gtc polling error");
                break;
            } else if (ret == 0) {// timeout handle
                printf("gtc polling time out");
                time_out_count++;
                if(time_out_count >= MAX_SOCKET_TIMEOUT) //MAX_SOCKET_TIMEOUT 次以上超时
                {
                    if(socket_init_status) //如果socket初始化过，关闭
                    {
                        close(sock_fd);
                    }
                    ret = InitSocket(); //重新初始化连接
                    if(ret != 0)        //初始化失败，重连
                    {
                        printf("socket init error");
                        sleep(1);
                        continue;
                    }   
                    else                //初始化成功，设置初始化状态，并接收消息
                    {
                        socket_init_status = true;
                        time_out_count = 0;
                        continue;
                    }      
                }
                continue;   
            }

            if (pfd.revents & POLLIN) {
                len = recv(sock_fd, &gtc_msg, sizeof(gtc_msg), 0);
                if (len < 0) {
                    if (errno == EAGAIN || errno == EWOULDBLOCK) {
                        /* no data to read, continue polling */
                        continue;
                    } else if (errno == ECONNREFUSED) {
                        // 连接被拒，通常是因为服务端没启动
                        perror("Connection refused, retrying...");
                        sleep(1); // 等待一秒后重试
                        continue; // 继续循环尝试重连
                    } else if (errno == ENOBUFS) {
                        // 缓冲区溢出，通常是因为服务端处理不过来
                        perror("Buffer overflow, retrying...");
                        sleep(1); // 等待一秒后重试
                        continue; // 继续循环尝试重连
                    } else
                    {
                        printf("gtc genl gtc msg recv failed");
                        break;
                    } 
                } else if (len == 0) {
                    printf("connection closed\n");
                    break;
                }

                if (gtc_msg.nlh.nlmsg_type == NLMSG_ERROR) {
                    printf("get gtc genl nlmsg type is NLMSG_ERROR\n");
                    err = (struct nlmsgerr *)NLMSG_DATA(&gtc_msg);
                    printf("ERR_NUM=%d - %s\n", err->error, strerror(-err->error));
                    break;
                }

                if (gtc_msg.nlh.nlmsg_type == family_id && gtc_msg.gnlh.cmd == GTC_CMD_SYNC_INFO) {
                    nla = (struct nlattr *)GENLMSG_DATA(&gtc_msg);
                    sync_info = (struct time_sync_parm *)NLA_DATA(nla);
                    curr_gtc_count = ((uint64_t)sync_info->gtc_hicnt << 32) | sync_info->gtc_lwcnt;
                    latch_gtc_hicnt = sync_info->latch_gtc_hicnt;
                    latch_gtc_lwcnt = sync_info->latch_gtc_lwcnt;
                    printf("receive gtc sync info: ");
                    printf("hicnt=%u lwcnt=%u ", sync_info->latch_gtc_hicnt, sync_info->latch_gtc_lwcnt); //gtc  + frequence
                    printf("sec=%lld nsec=%ld ", sync_info->aux_pps_sec, sync_info->aux_pps_nsec);//adas aux pps
                    printf("gtc_count=%lu\n", gtc_count);//gtc count
                    if (curr_gtc_count > gtc_count  && sync_info->aux_pps_sec >0) {
                        aux_pps_sec = sync_info->aux_pps_sec;
                        aux_pps_nsec = sync_info->aux_pps_nsec;
                        gtc_count = curr_gtc_count;
                    }
                    if(master_flag)
                    {
                        printf("timesync_msgbox_client_send sec:%ld nsec:%u\n", n_pps_sec, n_pps_nsec);
                        timesync_msgbox_client_send(n_pps_sec,n_pps_nsec);
                        PpsOutEnableSet();
                    }
                }
            }
        }

        close(sock_fd);
        m_gtcnl_thread_exit.store(true);
        timesync_msgbox_client_stop();
        return 0;
    }
}