/*
 * @Author: zjk 1553836110@qq.com
 * @Date: 2023-11-07 14:59:47
 * @LastEditors: zhaojingkui 1553836110@qq.com
 * @LastEditTime: 2023-11-30 11:58:46
 * @FilePath: /moos-ivp-pi/src/pEmulator/_150server.cpp
 * @Description: 
 * 
 * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
 */
#include "_150server.hpp"
#include <unistd.h>

uint16_t _150server::serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr)
{
    memcpy(bfr, &(embeddedInfo.header), sizeof(embeddedInfo.header));
    bfr += sizeof(embeddedInfo.header);

    memcpy(bfr, &(embeddedInfo.count), sizeof(embeddedInfo.count));
    bfr += sizeof(embeddedInfo.count);

    memcpy(bfr, &(embeddedInfo.size), sizeof(embeddedInfo.size));
    bfr += sizeof(embeddedInfo.size);    

    memcpy(bfr, &(embeddedInfo.drive_mode), sizeof(embeddedInfo.drive_mode));
    bfr += sizeof(embeddedInfo.drive_mode);

    memcpy(bfr, &(embeddedInfo.height), sizeof(embeddedInfo.height));
    bfr += sizeof(embeddedInfo.height);

    memcpy(bfr, &(embeddedInfo.depth), sizeof(embeddedInfo.depth));
    bfr += sizeof(embeddedInfo.depth);    

    memcpy(bfr, &(embeddedInfo.yaw), sizeof(embeddedInfo.yaw));
    bfr += sizeof(embeddedInfo.yaw);

    memcpy(bfr, &(embeddedInfo.pitch), sizeof(embeddedInfo.pitch));
    bfr += sizeof(embeddedInfo.pitch);

    memcpy(bfr, &(embeddedInfo.roll), sizeof(embeddedInfo.roll));
    bfr += sizeof(embeddedInfo.roll);    

    memcpy(bfr, &(embeddedInfo.ins_vx), sizeof(embeddedInfo.ins_vx));
    bfr += sizeof(embeddedInfo.ins_vx);

    memcpy(bfr, &(embeddedInfo.ins_vy), sizeof(embeddedInfo.ins_vy));
    bfr += sizeof(embeddedInfo.ins_vy);

    memcpy(bfr, &(embeddedInfo.ins_vz), sizeof(embeddedInfo.ins_vz));
    bfr += sizeof(embeddedInfo.ins_vz);            

    memcpy(bfr, &(embeddedInfo.lon), sizeof(embeddedInfo.lon));
    bfr += sizeof(embeddedInfo.lon);

    memcpy(bfr, &(embeddedInfo.lat), sizeof(embeddedInfo.lat));
    bfr += sizeof(embeddedInfo.lat);    

    memcpy(bfr, &(embeddedInfo.alt), sizeof(embeddedInfo.alt));
    bfr += sizeof(embeddedInfo.alt);

    memcpy(bfr, &(embeddedInfo.dvl_vx), sizeof(embeddedInfo.dvl_vx));
    bfr += sizeof(embeddedInfo.dvl_vx);

    memcpy(bfr, &(embeddedInfo.dvl_vy), sizeof(embeddedInfo.dvl_vy));
    bfr += sizeof(embeddedInfo.dvl_vy);    

    memcpy(bfr, &(embeddedInfo.dvl_vz), sizeof(embeddedInfo.dvl_vz));
    bfr += sizeof(embeddedInfo.dvl_vz);

    memcpy(bfr, &(embeddedInfo.rpm), sizeof(embeddedInfo.rpm));
    bfr += sizeof(embeddedInfo.rpm);

    memcpy(bfr, &(embeddedInfo.lightEnable), sizeof(embeddedInfo.lightEnable));
    bfr += sizeof(embeddedInfo.lightEnable); 

    memcpy(bfr, &(embeddedInfo.battery_voltage), sizeof(embeddedInfo.battery_voltage));
    bfr += sizeof(embeddedInfo.battery_voltage);

    memcpy(bfr, &(embeddedInfo.battery_level), sizeof(embeddedInfo.battery_level));
    bfr += sizeof(embeddedInfo.battery_level);    

    memcpy(bfr, &(embeddedInfo.battery_temp), sizeof(embeddedInfo.battery_temp));
    bfr += sizeof(embeddedInfo.battery_temp);

    memcpy(bfr, &(embeddedInfo.fault_leakSensor), sizeof(embeddedInfo.fault_leakSensor));
    bfr += sizeof(embeddedInfo.fault_leakSensor);

    memcpy(bfr, &(embeddedInfo.fault_battery), sizeof(embeddedInfo.fault_battery));
    bfr += sizeof(embeddedInfo.fault_battery); 

    memcpy(bfr, &(embeddedInfo.fault_emergencyBattery), sizeof(embeddedInfo.fault_emergencyBattery));
    bfr += sizeof(embeddedInfo.fault_emergencyBattery); 

    memcpy(bfr, &(embeddedInfo.fault_thrust), sizeof(embeddedInfo.fault_thrust));
    bfr += sizeof(embeddedInfo.fault_thrust);

    memcpy(bfr, &(embeddedInfo.iridium), sizeof(embeddedInfo.iridium));
    bfr += sizeof(embeddedInfo.iridium);    

    memcpy(bfr, &(embeddedInfo.throwing_load), sizeof(embeddedInfo.throwing_load));
    bfr += sizeof(embeddedInfo.throwing_load);

    memcpy(bfr, &(embeddedInfo.dvl_status), sizeof(embeddedInfo.dvl_status));
    bfr += sizeof(embeddedInfo.dvl_status);

    memcpy(bfr, &(embeddedInfo.crc), sizeof(embeddedInfo.crc));
    bfr += sizeof(embeddedInfo.crc); 

    memcpy(bfr, &(embeddedInfo.footer), sizeof(embeddedInfo.footer));
    bfr += sizeof(embeddedInfo.footer); 
}
void _150server::postInfo()
{     
    bzero(embeddedBuffer, 0);
    serializeFields(embeddedInfoSrc, embeddedBuffer);
    write(cfd, embeddedBuffer, 59);
};
bool _150server::listenInfo()
{   
    while (1)
    {
        int lens = read(cfd, recvbuf, sizeof(recvbuf));
        if(lens>0)
        {
            _150_recive++;
            uint16_t* buf16;
            printf("header: %u, %u\n", recvbuf[0], recvbuf[1]);
            printf("count: %u\n", recvbuf[2], recvbuf[3]);
            printf("size: %u\n", recvbuf[4]);
            printf("drive_mode: %u\n", recvbuf[5]);
            printf("thrust: %u\n", recvbuf[6]);
            _150cmd.thrust = (double)recvbuf[6];
            printf("yaw: %u, %u\n", recvbuf[7], recvbuf[8]);
            memcpy(buf16, &recvbuf[7], 2*sizeof(recvbuf[7]));
            _150cmd.yaw = *((double*)buf16);
            printf("depth: %u, %u\n", recvbuf[9], recvbuf[10]); 
            memcpy(buf16, &recvbuf[9], 2*sizeof(recvbuf[9]));
            _150cmd.depth = *((double*)buf16);
            printf("helm_top_angle: %u\n", recvbuf[11]);
            _150cmd.helm_top_angle = (double)recvbuf[11];
            printf("helm_bottom_angle: %u\n", recvbuf[12]);
            _150cmd.helm_bottom_angle = (double)recvbuf[12];
            _150cmd.helm_left_angle = (double)recvbuf[13];
            printf("helm_left_angle: %u\n", recvbuf[13]);
            _150cmd.helm_right_angle = (double)recvbuf[14];
            printf("helm_right_angle: %u\n", recvbuf[14]);
            _150cmd.light_enable = (double)recvbuf[15];
            printf("light_enable: %u\n", recvbuf[15]);
            printf("dvl_enable: %u\n", recvbuf[16]);
            _150cmd.dvl_enable = (double)recvbuf[16];
            printf("throwing_load_enable: %u\n", recvbuf[17]);
            _150cmd.throwing_load_enable = (double)recvbuf[16];
            printf("crc: %u\n", recvbuf[18]);
            _150cmd.crc = (double)recvbuf[18];
            printf("footer: %u, %u\n", recvbuf[19], recvbuf[20]);
            memcpy(buf16, &recvbuf[19], 2*sizeof(recvbuf[19]));
            _150cmd.footer = *((double*)buf16);
        }
    }
    return true;
}
void _150server::_150_startServer()
{
    std::cout << "--------------------" << std::endl;
    lfd = socket(AF_INET, SOCK_STREAM, 0);
    if(lfd==-1)
    {
        perror("socket");
        exit(-1);
    }
    //2.绑定
    struct sockaddr_in saddr;
    saddr.sin_family = PF_INET;
    saddr.sin_addr.s_addr = INADDR_ANY; //0.0.0.0
    saddr.sin_port = htons(8001);
    int ret = -1;
    do
    {
        ret = bind(lfd, (struct sockaddr *)&saddr, sizeof(saddr));
        std::cout << "Try bind adress..." << std::endl;
        sleep(1);
    } while (ret==-1);

    if(ret == -1)
    {
        perror("bind");
        exit(-1);
    }

    //3.监听
    do
    {
        listen(lfd, 5);
        std::cout << "Try to listening..." << std::endl;
        sleep(1);
    } while (ret==-1);

    if(ret==-1)
    {
        perror("listen");
        exit(-1);
    }
    //4.接受客户端连接
    struct sockaddr_in caddr;
    socklen_t len = sizeof(caddr);
    cfd = accept(lfd, (struct sockaddr *)&caddr, &len);

    if(cfd==-1)
    {
        perror("accept");
        exit(-1);
    }
    std::cout << "--------------------" << std::endl;
    //输出客户端的信息
    char cip[16];
    inet_ntop(AF_INET, &caddr.sin_addr.s_addr, cip, sizeof(cip));
    unsigned short cport = ntohs(caddr.sin_port);
    printf("client ip is %s,port is %d\n", cip, cport);
}