#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <pthread.h>
#include <errno.h>

#include "pxTools.h"
#include "pxEpoll.h"
#include "JMcan.h"
#include "canTransmit.h"
#include "JMPlc.h"

extern CONFIG_S config;
extern POSTDATABUF_S postData;
extern int epollfd;

static int serverfd=-1;
static int setMode=1;
static char ModeToSet=0;
static unsigned short packIndex=0;

static double wheelSet=0;
static int wheelFlag=1;
void PlcSetMode(int mode)
{
    ModeToSet=mode&0xff;
    if(mode==REMOTECONTROL)
        // postData.wheelSet=postData.wheelGet;
        wheelSet=0;
    setMode=1;
}
void wheelData(double *w)
{
    if(wheelFlag)
    {
        wheelSet=0;
        wheelFlag=0;
    }
    wheelSet+=*w;
    pxLog(DEBUG,"[PLC] set wheel[%2f] by %2f",wheelSet,*w);
}

int PlcSetWheel()
{
    static char buf[15]={0x00,0x00,0x00,0x00,0x00,0x09,0x01,0x10,0x01,0xA5,0x00,0x01,0x02,0x00,0x01};
    packIndex++;
    buf[0]=(packIndex>>8)&0xff;
    buf[1]=packIndex&0xff;
    // short wheel=(short)((*(double*)(&postData.wheelSet))*100);
    short wheel=(short)wheelSet;
    memcpy(&postData.wheelSet,&wheelSet,8);
    pxLog(DEBUG,"[PLC] send wheel [%d]",wheel);
    wheelFlag=1;
    buf[13]=(wheel>>8)&0xff;
    buf[14]=wheel&0xff;
    send(serverfd,buf,sizeof(buf),0);
    char retbuf[512]={0};
    int n=recv(serverfd,retbuf,sizeof(retbuf),0);
    if(n<=0)
        return -1;
    return 0;
}

int PlcGetWheel()
{
    // Tx:000131-06 79 00 00 00 06 01 03 01 F6 00 01
    // Rx:000132-06 79 00 00 00 05 01 03 02 00 00

    static char buf[12]={0x00,0x00,0x00,0x00,0x00,0x06,0x01,0x3,0x01,0xF6,0x00,0x01};
    packIndex++;
    buf[0]=(packIndex>>8)&0xff;
    buf[1]=packIndex&0xff;
    send(serverfd,buf,sizeof(buf),0);
    char retbuf[512]={0};
    int n=recv(serverfd,retbuf,sizeof(retbuf),0);
    if(n<=0)
        return -1;
    if(n>=11 && retbuf[7]==0x03)
    {
        short wheel=0;
        *((short*)&wheel)=retbuf[9]<<8 | retbuf[10];
        *((double*)&postData.wheelGet)=(double)wheel/100;
    }
    return 0;
}

int PlcGetLegs()
{
    // Tx:000131-06 79 00 00 00 06 01 03 02 04 00 02
    // Rx:000132-06 79 00 00 00 05 01 03 04 00 00 00 00

    static char buf[12]={0x00,0x00,0x00,0x00,0x00,0x06,0x01,0x3,0x02,0x08,0x00,0x07};
    packIndex++;
    buf[0]=(packIndex>>8)&0xff;
    buf[1]=packIndex&0xff;
    send(serverfd,buf,sizeof(buf),0);
    char retbuf[512]={0};
    int n=recv(serverfd,retbuf,sizeof(retbuf),0);
    if(n<=0)
        return -1;
    if(n>=23 && retbuf[7]==0x03)
    {
        memcpy(postData.plcdata,retbuf+9,sizeof(PLCRECVDATA_S));
        // short left=retbuf[9]<<8 | retbuf[10];
        // short right=retbuf[11]<<8 | retbuf[12];
        // postData.LeftLeg=(left/config.LeftLeg)*100;
        // postData.RightLeg=(right/config.RightLeg)*100;
    }
    return 0;
}

int sendMode()
{
    static char buf[15]={0x00,0x00,0x00,0x00,0x00,0x09,0x01,0x10,0x01,0x90,0x00,0x01,0x02,0x00,0x01};
    packIndex++;
    buf[0]=(packIndex>>8)&0xff;
    buf[1]=packIndex&0xff;
    buf[14]=ModeToSet;
    send(serverfd,buf,sizeof(buf),0);
    char retbuf[512]={0};
    int n=recv(serverfd,retbuf,sizeof(retbuf),0);
    if(n<=0)
        return -1;
    if(retbuf[7]==0x10)
        setMode=0;
    return 0;
}

void PLC_Run()
{
    //发送mode
    pxLog(INIT,"[PLC] PLC thread run");
    sendMode();
    while(1)
    {
        //发送mode
        //if(setMode)
        {
            if(sendMode()!=0)
            {
                timerAdd(epollfd,0,3000,0,PLC_conn,NULL);
                return;
            }
        }
        if(PlcGetWheel()!=0 || PlcGetLegs()!=0)
        {
            timerAdd(epollfd,0,3000,0,PLC_conn,NULL);
            return;
        }
        if(postData.status.system==REMOTECONTROL)
        {
            if(PlcSetWheel()!=0)
            {
                timerAdd(epollfd,0,3000,0,PLC_conn,NULL);
                return;
            }
        }
        usleep(40000);
    }
}

void PLC_conn(PXTIMER_S *p)
{
    pxLog(INIT,"[PLC] communication Init");
    postData.status.plc=0;
    postData.status.ros=0;
    serverfd=-1;
    if(config.PlcIP==NULL)
    {
        pxLog(ERROR,"[PLC] no pl ip");
        return;
    }
    int sock=0;
    struct sockaddr_in my_addr;
    if ((sock = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    {
        pxLog(ERROR,"[PLC] socket error:%d,retry in 3 sec",sock);
	    timerAdd(epollfd,0,3000,0,PLC_conn,NULL);
        return;
    }
    int mw_optval = 1;
    setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (char *)&mw_optval,sizeof(mw_optval));

    my_addr.sin_family = AF_INET;
    my_addr.sin_port = htons(502);
    if (inet_pton(AF_INET, config.PlcIP, &(my_addr.sin_addr)) <= 0) 
    {
        close(sock);
        pxLog(ERROR,"[PLC] plc ip parse error");
        return;
    }
    postData.status.ros=1;
    bzero(&(my_addr.sin_zero),8);
    // 设置连接超时时间
	struct timeval timeout={2,0};
    if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout)) < 0 || setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)))
    {
        close(sock);
        pxLog(ERROR,"[PLC] socket set timeout error,retry in 3 sec");
	    timerAdd(epollfd,0,3000,0,PLC_conn,NULL);
        return;
    }

    // 连接到服务器
    if (connect(sock, (struct sockaddr*)&my_addr, sizeof(my_addr)) == -1) 
    {
        close(sock);
        pxLog(ERROR,"[PLC] connect error");
	    timerAdd(epollfd,0,3000,0,PLC_conn,NULL);
        return;
    }
    serverfd=sock;
    pthread_t thread;
    pthread_create(&thread, NULL, PLC_Run, NULL);
}