#include "goose_HW.h"

// #include <sys/ioctl.h >
// #include <net/if.h >


static char* format_mac(char* buf, uint8_t* mac)
{
    sprintf(buf, "%02x %02x %02x %02x %02x %02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
    return buf;
}

static char* format_typestring(uint16_t type)
{
    switch(type)
    {
        case 1: return "PRT_ACDC_PLACEHOLDER";
        case 2: return "PRT_INSLUATION_DETECTION_START";
        case 3: return "PRT_INSLUATION_DETECTION_STOP";
        case 4: return "PRT_PRECHARGING";
        case 5: return "PRT_CHARGING";
        default: return "unknow";
    }
    return "unknow";
}

int goose_HW_open_socket(goose_HW_t * dev)
{
    if (dev && dev->fd < 0)
    {
        dev->fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
        if (dev->fd > 0)
        {
            memset(&dev->sll, 0, sizeof(dev->sll));
            dev->sll.sll_family = PF_PACKET;
            dev->sll.sll_ifindex = if_nametoindex(dev->interfaceID);/*sll虽然是用来指定目的地址的，但是在这个结构体中sll_ifindex 却指定的是本机发送报文的接口索引*/
            dev->sll.sll_protocol = htons(0x80B8);
            dev->slen = sizeof(dev->sll);
            if (bind(dev->fd, (struct sockaddr*)&dev->sll, dev->slen)==0)
            {
                struct ifreq stIf;
                sprintf(stIf.ifr_name, "%s", dev->interfaceID);
                if (ioctl(dev->fd, SIOCGIFINDEX, &stIf)==0)
                {
                    if (ioctl(dev->fd, SIOCGIFHWADDR, &stIf)==0)
                    {
                        memcpy(dev->src_mac, stIf.ifr_hwaddr.sa_data, 6);
                        printi("mac:%02x %02x %02x %02x %02x %02x\n", dev->src_mac[0], dev->src_mac[1], dev->src_mac[2], dev->src_mac[3], dev->src_mac[4], dev->src_mac[5]);
                        return 0;
                    }
                    printe("ioctl(SIOCGIFHWADDR, fd=%d) error\n", dev->fd);
                    return -5;
                }
                printe("ioctl(SIOCGIFINDEX, fd=%d) error\n", dev->fd);
                return -4;
            }
            printe("bind(fd=%d) error\n", dev->fd);
            return -3;
        }
        printe("socket(fd=%d) error\n", dev->fd);
        return -2;
    }
    return -1;
}

static int goose_HW_recv_with_timeout(goose_HW_t* dev, int timeout)
{
    int ret, readb;
    fd_set rset;
    struct timeval tv;
    char strbuf[128];
    if (dev)
    {
        dev->rx_bytes = 0;
        FD_ZERO(&rset);
        FD_SET(dev->fd, &rset);
        tv.tv_sec = timeout/1000;
        tv.tv_usec = (timeout%1000) * 1000;
        if(select(dev->fd + 1, &rset, NULL, NULL, &tv)>0)
        {
            if(FD_ISSET(dev->fd, &rset))
            {
                {
                    memset(&dev->sll, 0, sizeof(dev->sll));
                    dev->sll.sll_family = PF_PACKET;
                    dev->sll.sll_ifindex = if_nametoindex(dev->interfaceID);/*sll虽然是用来指定目的地址的，但是在这个结构体中sll_ifindex 却指定的是本机发送报文的接口索引*/
                    dev->sll.sll_protocol = htons(0x808B); 
                    dev->sll.sll_addr[0] = dev->src_mac[0];/*sll_addr指定目的MAC地址*/
                    dev->sll.sll_addr[1] = dev->src_mac[1];
                    dev->sll.sll_addr[2] = dev->src_mac[2];
                    dev->sll.sll_addr[3] = dev->src_mac[3];
                    dev->sll.sll_addr[4] = dev->src_mac[4];
                    dev->sll.sll_addr[5] = dev->src_mac[5];
                    dev->slen = sizeof(dev->sll);
                }
                // readb = recv(dev->fd, &dev->rx_buff[dev->rx_bytes], (1536 - dev->rx_bytes), 0);
                readb = recvfrom(dev->fd, &dev->rx_buff[dev->rx_bytes], (1536 - dev->rx_bytes), 0, (struct sockaddr*)&dev->sll, &dev->slen);
                // readb = recvfrom(dev->fd, &dev->rx_buff[dev->rx_bytes], (1536 - dev->rx_bytes), 0, NULL, NULL);
                if (readb>0)
                {
                    dev->rx_bytes += readb;
                    if (dev->rx_bytes>0) printline("recv", dev->rx_buff, dev->rx_bytes);
                    return 0;
                }
                printe("src:%s, dst:%s, ret:%d\n",format_mac(&strbuf[64], dev->src_mac), format_mac(strbuf, dev->dst_mac), readb);
                return -4;
            }
            printe("src:%s, dst:%s, ret:%d\n",format_mac(&strbuf[64], dev->src_mac), format_mac(strbuf, dev->dst_mac), readb);
            return -3;
        }
        printe("src:%s, dst:%s\n",format_mac(strbuf, dev->src_mac), format_mac(strbuf, dev->dst_mac));
        return -2;
    }
    return -1;
}

static int goose_HW_send(goose_HW_t* dev)
{
    int sendb;
    char strbuf[128];
    if (dev && dev->fd > 0)
    {
        if (dev->tx_bytes > 0)
        {
            {
                memset(&dev->sll, 0, sizeof(dev->sll));
                dev->sll.sll_family = PF_PACKET;
                dev->sll.sll_ifindex = if_nametoindex(dev->interfaceID);/*sll虽然是用来指定目的地址的，但是在这个结构体中sll_ifindex 却指定的是本机发送报文的接口索引*/
                dev->sll.sll_protocol = htons(0x808B); 
                dev->sll.sll_addr[0] = dev->dst_mac[0];/*sll_addr指定目的MAC地址*/
                dev->sll.sll_addr[1] = dev->dst_mac[1];
                dev->sll.sll_addr[2] = dev->dst_mac[2];
                dev->sll.sll_addr[3] = dev->dst_mac[3];
                dev->sll.sll_addr[4] = dev->dst_mac[4];
                dev->sll.sll_addr[5] = dev->dst_mac[5];
                dev->slen = sizeof(dev->sll);
            }
            //printline("\ngoose send\n", &dev->tx_buff[0], dev->tx_bytes); printf("\n");
            // sendb = send(dev->fd, &dev->tx_buff[0], dev->tx_bytes, 0);
            sendb = sendto(dev->fd, &dev->tx_buff[0], dev->tx_bytes, 0, (struct sockaddr*)&dev->sll, dev->slen);
            if (sendb > 0)
            {
                int remain_b = dev->tx_bytes - sendb;
                memmove(&dev->tx_buff[0], &dev->tx_buff[sendb], remain_b);
                dev->tx_bytes = remain_b;
                return 0;
            }
            printe("src:%s, dst:%s, ret:%d\n",format_mac(&strbuf[64], dev->src_mac), format_mac(strbuf, dev->dst_mac), sendb);
            close(dev->fd);
            dev->fd = -1;
            return -3;
        }
        return -2;
    }
    return -1;
}

static int goose_HW_PowerRequest_send(goose_HW_t* dev, PowerRequest_t* pdu)
{
    uint8_t buff0[1536];
    uint8_t buff1[1536];

    int index0=0;
    int index1=0;

    pthread_mutex_lock(&dev->mutex);
    dev->sqNum++;
    dev->goCBRef = "CC/0$GO$PowerRequest";
    dev->timeAllowedToLive = 10000;
    dev->dataSetRef = "CC/0$GO$PowerRequest";
    dev->goID = "CC/0$GO$PowerRequest";
    dev->simulation = false;
    dev->needsCommission = false;
    dev->confRev = 1;

    printd("goose:%p, ST:%d, SQ:%d, type:%s, V:%9f V, I:%9f A\n", dev, dev->stNum, dev->sqNum, format_typestring(dev->power_request.type),dev->power_request.requestV,dev->power_request.requestI);

    gettimeofday(&dev->timestamp, NULL);
    index0 = goose_HW_encode_PowerRequest(dev, buff0, index0, pdu);
    index1 = goose_HW_encode_APDU(dev, buff1, index1, 8, buff0, index0);
    index0 = goose_HW_encode_TLV(buff0, 0, 0x61, index1, buff1);
    dev->tx_bytes = goose_HW_encode_Eth8021Q(dev, dev->tx_buff, 0, 5, 0x0000, 0x0001, buff0, index0);
    goose_HW_send(dev);
    pthread_mutex_unlock(&dev->mutex);
    return dev->tx_bytes;
}

static int goose_HW_ShuntdownRequest_send(goose_HW_t* dev, ShutdownRequest_t* pdu)
{
    uint8_t buff0[1536];
    uint8_t buff1[1536];
    int index0=0;
    int index1=0;

    pthread_mutex_lock(&dev->mutex);
    dev->sqNum++;
    dev->goCBRef = "CC/0$GO$ShutdownRequest";
    dev->timeAllowedToLive = 10000;
    dev->dataSetRef = "CC/0$GO$ShutdownRequest";
    dev->goID = "CC/0$GO$ShutdownRequest";
    dev->simulation = false;
    dev->needsCommission = false;
    dev->confRev = 1;

    //printd("goose:%p, ST:%d, SQ:%d, reason:%hd\n", dev, dev->stNum, dev->sqNum, dev->shutdown_request.reason);

    gettimeofday(&dev->timestamp, NULL);
    index0 = goose_HW_encode_ShuntdownRequest(dev, buff0, index0, pdu);
    index1 = goose_HW_encode_APDU(dev, buff1, index1, 5, buff0, index0);
    index0 = goose_HW_encode_TLV(buff0, 0, 0x61, index1, buff1);
    dev->tx_bytes = goose_HW_encode_Eth8021Q(dev, dev->tx_buff, 0, 5, 0x0000, 0x3002, buff0, index0);
    goose_HW_send(dev);
    pthread_mutex_unlock(&dev->mutex);
    return dev->tx_bytes;
}

static int goose_HW_wait_respose(goose_HW_t* dev, int64_t timeout)
{
    int64_t deadline_ms = systime_ms() + timeout;
    for(; timeout>0; timeout = deadline_ms - systime_ms())
    {
        // msleep(timeout);
        // printd("goose:%p, timeout:%lld, systime:%lld, state:%d\n", dev, timeout, systime_ms()/1000, dev->state);
        goose_HW_recv_with_timeout(dev, timeout);

        while (dev->rx_bytes >= 18)
        {
            int index = 12;
            uint8_t* buff = dev->rx_buff;
            //goose_HW_decode_Eth8021Q
            if (buff[index+0]==0x81 && buff[index+1]==0x00)
            {
                uint16_t priority = (buff[index+2]>>5)&0x7;
                uint16_t VLAN = ((buff[index+2]&0xF)<<8)|buff[index+3];
                index += 4;
            }

            if (buff[index+0]==0x88 && buff[index+1]==0xB8)
            {
                if (dev->rx_bytes >= (index+6))
                {
                    uint16_t APPID = (buff[index+2]<<8)|buff[index+3];
                    uint16_t length = ((buff[index+4]<<8)|buff[index+5]) - 8;
                    index = index + 10 + length;
                    printe("APPID:%04x, length:%04x, src:%02x %02x %02x %02x %02x %02x, dst:%02x %02x %02x %02x %02x %02x\n",APPID, length, buff[0], buff[1],buff[2],buff[3],buff[4],buff[5],buff[6],buff[7],buff[8],buff[9],buff[10],buff[11] ); 
                    if (dev->rx_bytes >= index)
                    {
                        if (buff[0]==dev->src_mac[0] && buff[6] ==dev->dst_mac[0]
                            && buff[1]==dev->src_mac[1] && buff[7] ==dev->dst_mac[1]
                            && buff[2]==dev->src_mac[2] && buff[8] ==dev->dst_mac[2]
                            && buff[3]==dev->src_mac[3] && buff[9] ==dev->dst_mac[3]
                            && buff[4]==dev->src_mac[4] && buff[10]==dev->dst_mac[4]
                            && buff[5]==dev->src_mac[5] && buff[11]==dev->dst_mac[5])
                        {
                            printd("goose:%p, APPID:%04x, length:%04x\n", dev, APPID, length);
                            switch(APPID)
                            {
                                case 0x3001: goose_HW_decode_APDU(dev, &buff[24], length); break;
                                case 0x3003: goose_HW_decode_APDU(dev, &buff[24], length); break;
                            }
                        }  
                        // printline("recv", dev->rx_buff, dev->rx_bytes);
                        memmove(&buff[0], &buff[index], dev->rx_bytes - index);
                        dev->rx_bytes -= index;
                    }
                }
            }else{
                //not goose message, next byte
                memmove(&buff[0], &buff[1], dev->rx_bytes-1);
                dev->rx_bytes -= 1;
            }
        }
    }
    return -1;
}


int goose_HW_init(goose_HW_t * dev, const char* interfaceID, uint16_t connector_id)
{
    if (dev)
    {
        pthread_mutex_init(&dev->mutex, NULL);
        
        dev->quit = 0;
        dev->T0 = 5000;
        dev->T1 = 1000;
        dev->T[0] = 5000;
        dev->T[1] = 2000;
        dev->T[2] = 2000;
        dev->T[3] = 4000;
        dev->T[4] = 8000;
        
        dev->state = GHWS_POWER_REQUEST;
        dev->power_request.connect_id = connector_id;
        dev->power_request.serialnumb = 0;
        dev->power_request.type = 1;
        dev->power_request.mode = 2;
        dev->power_request.requestI=100.0;
        dev->power_request.requestV=1000.0;
        dev->power_request.resv0=0xFFFF;
        dev->power_request.resv1=0xFFFF;

        dev->shutdown_request.connect_id = connector_id;
        dev->shutdown_request.serialnumb = 0;
        dev->shutdown_request.reason = 0;
        dev->shutdown_request.resv1=0xFFFF;
        dev->shutdown_request.resv1=0xFFFF;

        // 20:53:83:5c:63:9c
        dev->dst_mac[0] = 0x20;
        dev->dst_mac[1] = 0x53;
        dev->dst_mac[2] = 0x83;
        dev->dst_mac[3] = 0x5c;
        dev->dst_mac[4] = 0x63;
        dev->dst_mac[5] = 0x9c;

        dev->interfaceID = interfaceID;
        dev->fd = -1;
        return goose_HW_open_socket(dev);
    }
    return -1;
}

void goose_HW_PowerRequest(goose_HW_t * dev)
{
    if (dev)
    {
        pthread_mutex_lock(&dev->mutex);
        dev->stNum++;
        dev->sqNum=0;
        dev->state = GHWS_POWER_REQUEST;
        
        
        dev->T[0] = dev->T0;
        dev->T[1] = dev->T1;
        dev->T[2] = dev->T1*2;
        dev->T[3] = dev->T1*4;
        dev->T[4] = dev->T1*8;
        dev->T_idx = 0;

        pthread_mutex_unlock(&dev->mutex);

        // printi("HMAC:%02x %02x %02x %02x %02x %02x %02x %02x\n", dev->HMAC[0], dev->HMAC[1], dev->HMAC[2], dev->HMAC[3], dev->HMAC[4], dev->HMAC[5], dev->HMAC[6], dev->HMAC[7]);

        goose_HW_PowerRequest_send(dev, &dev->power_request);
    }
}

void goose_HW_ShuntdownRequest(goose_HW_t * dev)
{
    if (dev)
    {
        pthread_mutex_lock(&dev->mutex);
        dev->stNum++;
        dev->sqNum=0;
        dev->state = GHWS_SHUTDOWN_REQUEST;

        dev->T[0] = dev->T0;
        dev->T[1] = dev->T1;
        dev->T[2] = dev->T1*2;
        dev->T[3] = dev->T1*4;
        dev->T[4] = dev->T1*8;
        dev->T_idx = 0;
        pthread_mutex_unlock(&dev->mutex);

        goose_HW_ShuntdownRequest_send(dev, &dev->shutdown_request);
    }
}

void* goose_HW_loop(void* argv)
{
    goose_HW_t* dev = (goose_HW_t*)argv;
    if (dev)
    {
        while (dev->quit == 0)
        {
            goose_HW_open_socket(dev);

            // printd("goose:%p, ST:%d, SQ:%d\n", dev, dev->stNum, dev->sqNum);
            
            switch (dev->state)
            {
            case GHWS_POWER_REQUEST: goose_HW_PowerRequest_send(dev, &dev->power_request); break;
            case GHWS_SHUTDOWN_REQUEST: goose_HW_ShuntdownRequest_send(dev, &dev->shutdown_request); break;
            }

            // printd("goose:%p, ST:%d, SQ:%d\n", dev, dev->stNum, dev->sqNum);

            pthread_mutex_lock(&dev->mutex);
            dev->T_idx = dev->T_idx + 1;
            if (dev->T_idx >= 4) dev->T_idx = 4;
            pthread_mutex_unlock(&dev->mutex);

            // printd("goose:%p, ST:%d, SQ:%d\n", dev, dev->stNum, dev->sqNum);

            if (dev->T_idx == 4){
                msleep(dev->T[0]);
                // goose_HW_wait_respose(dev, dev->T[0]);
            }else{
                msleep(dev->T[dev->T_idx]);
                // goose_HW_wait_respose(dev, dev->T[dev->T_idx]);
            }
        }
    }
    return NULL;
}

void goose_HW_exit(goose_HW_t * dev)
{
    if (dev)
    {
        pthread_mutex_destroy(&dev->mutex);
        close(dev->fd);
    }
}