#include "v2g_context.h"

const char* v2g_msg_type[] = {
    "Supported App Protocol",
    "Session Setup",
    "Service Discovery",
    "Service Detail",
    "Payment Service Selection",
    "Payment Details",
    "Authorization",
    "Charge Parameter Discovery",
    "Metering Receipt",
    "Certificate Update",
    "Certificate Installation",
    "Charging Status",
    "Cable Check",
    "Pre Charge",
    "Power Delivery",
    "Current Demand",
    "Welding Detection",
    "Session Stop",
    "Unknown",
};

void v2g_connection_free(v2g_context_t* v2g, v2g_connect_t* conn)
{
    if (v2g->next_client == conn && conn!=NULL)
    {
        v2g->next_client = NULL;
        conn->v2g_connection_exit(conn);
        // free(conn);
        return;
    }

    // v2g_connect_t *temp = (v2g_connect_t*)v2g->next_client;
    // while(temp->next_client!=NULL)
    // {
    //     if (temp->next_client == conn)
    //     {
    //         temp->next_client = conn->next_client;
    //         conn->v2g_connection_exit(conn);
    //         free(conn);
    //         break;
    //     }
    // }
}

static int v2g_incoming_v2gtp(v2g_connect_t* conn)
{
    uint16_t payloadType = V2GTP20_SAP_PAYLOAD_ID;
    uint32_t payloadLength;
    uint32_t V2GTP_FRAME_LENGTH = V2GTP_HEADER_LENGTH + payloadLength;
    memset(conn->rxbuff, 0, DEFAULT_BUFFER_SIZE);
    conn->rxlen = 0;
    while(conn->rxlen < V2GTP_HEADER_LENGTH)
    {
        int rv = conn->v2g_connection_recv(conn, &conn->rxbuff[conn->rxlen], V2GTP_HEADER_LENGTH - conn->rxlen, 0xFFFFFFFF);
        if (rv>0)
        {
            conn->rxlen += rv;
        }else{
            return -1;
        }
        if (conn->rxlen == V2GTP_HEADER_LENGTH)
        {
            if (V2GTP_ReadHeader(conn->rxbuff, &payloadLength) == 0)
            {
                payloadType = V2GTP20_SAP_PAYLOAD_ID;
                V2GTP_FRAME_LENGTH = V2GTP_HEADER_LENGTH + payloadLength;
                if (V2GTP_FRAME_LENGTH <= DEFAULT_BUFFER_SIZE)
                {
                    break;
                }
            }
            memmove(&conn->rxbuff[0], &conn->rxbuff[1], V2GTP_HEADER_LENGTH-1);
            conn->rxlen -= 1;
        }
    }
    // printline(conn->rxbuff, conn->rxlen, "incoming.header.frame_len(%d)", V2GTP_FRAME_LENGTH);
    while(conn->rxlen < V2GTP_FRAME_LENGTH)
    {
        int rv = conn->v2g_connection_recv(conn, &conn->rxbuff[conn->rxlen], V2GTP_FRAME_LENGTH - conn->rxlen, 0xFFFFFFFF);
        if (rv>0)
        {
            conn->rxlen += rv;
        }else{
            printline(conn->rxbuff, conn->rxlen, "incoming.error(%d. %d).frame_len(%d)", rv, errno, V2GTP_FRAME_LENGTH);
            return -1;
        }
        if (conn->rxlen == V2GTP_FRAME_LENGTH)
        {
            conn->stream.byte_pos = V2GTP_HEADER_LENGTH;
            conn->stream.data_size = V2GTP_FRAME_LENGTH;
            // printline(conn->rxbuff, conn->rxlen, "incoming");
            return payloadType;
        }
    }
    printline(conn->rxbuff, conn->rxlen, "incoming.frame.frame_len(%d)", V2GTP_FRAME_LENGTH);
    return -1;
}

static int v2g_outgoing_v2gtp(v2g_connect_t* conn)
{
    /* fixup/create header */
    int len = exi_bitstream_get_length(&conn->stream);

    V2GTP_WriteHeader(conn->rxbuff, len - V2GTP_HEADER_LENGTH);

    // printline(conn->rxbuff, len, "outgoing");

    if (conn->v2g_connection_send(conn, conn->rxbuff, len) == -1) 
    {
        mlog_e(LOG_TAG_V2G, "connection_write(header) failed: %s", strerror(errno));
        return -1;
    }

    return 0;
}

static int v2g_handle_apphandshake(struct v2g_connect* conn)
{
    int i;
    
    mlog_d(LOG_TAG_V2G, "Handling SupportedAppProtocolReq");
    if (decode_appHand_exiDocument(&conn->stream, &conn->handshake_req) != 0) {
        mlog_e(LOG_TAG_V2G, "decode_appHandExiDocument() failed");
        return -2; // If the mesage can't be decoded we have to terminate the tcp-connection  (e.g. after an unexpected message)
    }

    init_appHand_exiDocument(&conn->handshake_resp);
    conn->handshake_resp.supportedAppProtocolRes_isUsed = 1;
    conn->handshake_resp.supportedAppProtocolRes.ResponseCode = appHand_responseCodeType_Failed_NoNegotiation; // [V2G2-172]
    for (i = 0; i < conn->handshake_req.supportedAppProtocolReq.AppProtocol.arrayLen; i++) 
    {
        struct appHand_AppProtocolType* app_proto = &conn->handshake_req.supportedAppProtocolReq.AppProtocol.array[i];
        app_proto->ProtocolNamespace.characters[app_proto->ProtocolNamespace.charactersLen] = '\0';
        mlog_d(LOG_TAG_V2G, "EVCC.supportedAppProtocolReq[%d]:%s, Priority:%d\n", i, app_proto->ProtocolNamespace.characters, app_proto->Priority);
        if (strncmp(DIN_70121_2012_MSG_DEF, app_proto->ProtocolNamespace.characters, app_proto->ProtocolNamespace.charactersLen) == 0)
        {
            conn->handshake_resp.supportedAppProtocolRes.ResponseCode = appHand_responseCodeType_OK_SuccessfulNegotiation;
            conn->handshake_resp.supportedAppProtocolRes.SchemaID = app_proto->SchemaID;
            conn->handshake_resp.supportedAppProtocolRes.SchemaID_isUsed = (unsigned int)1;
            conn->selected_protocol = V2G_PROTO_DIN70121_2012;
            mlog_d(LOG_TAG_V2G, "Protocol negotiation was successful. Selected protocol is DIN70121_2012");
            return V2G_PROTO_DIN70121_2012;
        }
        if (strncmp(ISO_15118_2013_MSG_DEF, app_proto->ProtocolNamespace.characters, app_proto->ProtocolNamespace.charactersLen) == 0)
        {
            conn->handshake_resp.supportedAppProtocolRes.ResponseCode = appHand_responseCodeType_OK_SuccessfulNegotiation;
            conn->handshake_resp.supportedAppProtocolRes.SchemaID = app_proto->SchemaID;
            conn->handshake_resp.supportedAppProtocolRes.SchemaID_isUsed = (unsigned int)1;
            conn->selected_protocol = V2G_PROTO_ISO15118_2013;
            mlog_d(LOG_TAG_V2G, "Protocol negotiation was successful. Selected protocol is ISO15118_2013");
            return V2G_PROTO_ISO15118_2013;
        }
    }
    return -1;
}

void* v2g_connection_loop(void* args)
{
    v2g_connect_t* conn = (v2g_connect_t*)args;
    v2g_context_t* v2g = conn->v2g;
    exi_bitstream_init(&conn->stream, conn->rxbuff, 0, 0, NULL);
    while(conn->quit == 0)
    {
        int rv = v2g_incoming_v2gtp(conn);
        if (rv == V2GTP20_SAP_PAYLOAD_ID)
        {
            if (v2g_handle_apphandshake(conn) > 0)
            {
                break;
            }
            continue;
        }
        return NULL;
    }
    /* encode response at the right buffer location */
    conn->stream.byte_pos = V2GTP_HEADER_LENGTH;
    conn->stream.bit_count = 0;
    if (encode_appHand_exiDocument(&conn->stream, &conn->handshake_resp) != 0) 
    {
        mlog_e(LOG_TAG_V2G, "Encoding of the protocol handshake message failed");
        return NULL;
    }
    if (v2g_outgoing_v2gtp(conn))
    {
        mlog_e(LOG_TAG_V2G, "Encoding of the protocol handshake message failed");
        return NULL;
    }

    while(conn->quit == 0)
    {
        int event;
        int exirv;
        int outrv;
        int xinrv = v2g_incoming_v2gtp(conn);
        exi_bitstream_init(&conn->stream, conn->rxbuff, 0, 0, NULL);
        conn->stream.byte_pos = V2GTP_HEADER_LENGTH;
        conn->stream.data_size = conn->rxlen;
        if (xinrv == V2GTP20_SAP_PAYLOAD_ID)
        {
            switch(conn->selected_protocol)
            {
                case V2G_PROTO_DIN70121_2012: {event = v2g_handle_din_request(conn);}break;
                case V2G_PROTO_ISO15118_2013: {event = v2g_handle_iso_request(conn);}break;
            }

            switch (event) 
            {
                case V2G_EVENT_SEND_AND_TERMINATE: conn->quit = 1;
                case V2G_EVENT_NO_EVENT:{
                    conn->stream.data[0] = 0;
                    conn->stream.bit_count = 0;
                    conn->stream.byte_pos = V2GTP_HEADER_LENGTH;
                    conn->stream.data_size = DEFAULT_BUFFER_SIZE;
                    switch(conn->selected_protocol)
                    {
                        case V2G_PROTO_DIN70121_2012: {exirv = encode_din_exiDocument(&conn->stream, &conn->dinEXIDocument_out);}break;
                        case V2G_PROTO_ISO15118_2013: {exirv = encode_iso2_exiDocument(&conn->stream, &conn->iso2EXIDocument_out);}break;
                    }
                }

                case V2G_EVENT_SEND_RECV_EXI_MSG: {
                    if (exirv==0) outrv = v2g_outgoing_v2gtp(conn); 
                    if (exirv!=0 || outrv!=0) mlog_e(LOG_TAG_V2G, "v2g_outgoing_v2gtp() \"%s\" failed: %d", v2g_msg_type[conn->v2g->current_v2g_msg], outrv);
                }break;

                case V2G_EVENT_IGNORE_MSG: {
                    mlog_e(LOG_TAG_V2G, "Ignoring V2G request message \"%s\". Waiting for next request", v2g_msg_type[conn->v2g->current_v2g_msg]); 
                }break;

                case V2G_EVENT_TERMINATE_CONNECTION:
                default:  {
                    conn->quit = 1;
                    mlog_e(LOG_TAG_V2G, "Failed to handle V2G request message \"%s\"", v2g_msg_type[conn->v2g->current_v2g_msg]); 
                }break;
            }
        }
    }
    return NULL;
}