#include "pcm.h"
#include "facilities.h"

#include <it2s-tender/epv.h>
#include <it2s-tender/space.h>
#include <it2s-tender/trajectory.h>
#include <it2s-tender/geodesy.h>
#include <itss-transport/TransportRequest.h>
#include <pcm/PCM.h>

#define SLEEP_TIME_MS 100

static int are_vehicles_intersecting(
        it2s_tender_st_s* tA, int tA_len, uint16_t vA_length, uint16_t vA_width, 
        it2s_tender_st_s* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
        int* index
        ) {

    // TODO check first time intersection, then the spacial trajectory
    double A1[2], A2[2], B1[2], B2[2];
    uint64_t tsA, tsB;
    for (int a = 0; a < tA_len; ++a) {
//    for (int a = 0; a < tA_len-1; ++a) {
//        A1[0] = tA[a].latitude;
//        A1[1] = tA[a].longitude;
//        A2[0] = tA[a+1].latitude;
//        A2[1] = tA[a+1].longitude;


        for (int b = 0; b < tB_len; ++b) {
//        for (int b = 0; b < tB_len-1; ++b) {
//            B1[0] = tB[b].latitude;
//            B1[1] = tB[b].longitude;
//            B2[0] = tB[b+1].latitude;
//            B2[1] = tB[b+1].longitude;

//            if (it2s_tender_do_segments_intersect(A1, A2, B1, B2)) {
            if (it2s_tender_is_inside_rectangle(tA[a].latitude, tA[a].longitude, tB[b].latitude, tB[b].longitude, 8, 8, 0, DCM_HAVERSINE)) {
                if (tA[a].timestamp < tB[b].timestamp + 2000 &&
                    tA[a].timestamp > tB[b].timestamp - 2000) {
                    *index = a;
                    return 1;
                }
            }
        }
    }

    return 0;
}

static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm) {
    int rv = 0;

    CoordinationRequest_t* request = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.request;

    PCM_t* pcm_rep = NULL;
    TransportRequest_t* tr = NULL;

    const ssize_t buf_len = 512;
    uint8_t buf[buf_len];

    it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
    ssize_t trajectoryA_len = 0;

    uint64_t now = it2s_tender_get_clock(&facilities->epv);

    int32_t lat, lon;
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    lat = facilities->epv.space.latitude;
    lon = facilities->epv.space.longitude;
    it2s_tender_unlock_space(&facilities->epv);

    it2s_tender_lock_trajectory(&facilities->epv);
    trajectoryA_len = facilities->epv.trajectory.len;
    memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
    it2s_tender_unlock_trajectory(&facilities->epv);

    trajectoryA[0].latitude = lat;
    trajectoryA[0].longitude = lon;
    trajectoryA[0].timestamp = now;
    ++trajectoryA_len;

    pcm_rep = calloc(1, sizeof(PCM_t));

    pcm_rep->header.messageID = 43;
    pcm_rep->header.protocolVersion = 1;
    pcm_rep->header.stationID = 1;
    pthread_mutex_lock(&facilities->id.lock);
    pcm_rep->header.stationID = facilities->id.station_id;
    pthread_mutex_unlock(&facilities->id.lock);
            
    pcm_rep->pcm.currentPosition.latitude = lat;
    pcm_rep->pcm.currentPosition.longitude = lon;
    asn_ulong2INTEGER(&pcm_rep->pcm.currentPosition.timestamp, now);

    pcm_rep->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
    ManeuverVehicleContainer_t* mvc = &pcm_rep->pcm.maneuverContainer.choice.vehicle;

    // Vehicle DimensionsDimensions
    mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
    mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
    mvc->vehicleWidth = facilities->vehicle.width;

    // Accepted trajectory
    if (!request->desiredTrajectories.list.count) {
        syslog_debug("[facilities] [pc] received PCM request has no desired trajectories");
        rv = 1;
        goto cleanup;
    }
    ProposedTrajectory_t* pt = request->desiredTrajectories.list.array[0];

    mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
    mvc->negotiation->present = CoordinationNegotiation_PR_reply;
    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1;
    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long));
    *mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id;

    asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_PCM, pcm_rep, buf, buf_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [pc] PCM request encode failure");
        rv = 1;
        goto cleanup;
    }

    tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;
    tr->choice.packet.present = TransportPacketRequest_PR_btp;
    BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;

    bpr->btpType = BTPType_btpB;

    bpr->gn.destinationAddress.buf = malloc(6);
    for (int i = 0; i < 6; ++i) {
        bpr->gn.destinationAddress.buf[i] = 0xff;
    }
    bpr->gn.destinationAddress.size = 6;

    bpr->gn.packetTransportType = PacketTransportType_shb;

    bpr->destinationPort = 2043;

    bpr->gn.trafficClass = 2;

    bpr->data.buf = malloc(512);
    memcpy(bpr->data.buf, buf, buf_len);
    bpr->data.size = (enc.encoded + 7) / 8;

    buf[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [pc] TR PCM request encode failure");
        rv = 1;
        goto cleanup;
    }

    queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_PCM, pcm_rep);
    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    return rv;
}

static int pcm_check_handle_reply(facilities_t* facilities, PCM_t* pcm) {
    int rv = 0;

    CoordinationReply_t* reply = &pcm->pcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;

    syslog_info("[facilities] [pc] received coordination reply with %d accepted trajectories", reply->acceptedTrajectoriesIds.list.count);

    return rv;
}

static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm) {

    int rv = 0;

    PCM_t* pcm_req = NULL;
    TransportRequest_t* tr = NULL;

    const ssize_t buf_len = 512;
    uint8_t buf[buf_len];

    it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
    uint16_t trajectoryA_len = 0;

    int32_t lat, lon;
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    lat = facilities->epv.space.latitude;
    lon = facilities->epv.space.longitude;
    it2s_tender_unlock_space(&facilities->epv);

    it2s_tender_lock_trajectory(&facilities->epv);
    trajectoryA_len = facilities->epv.trajectory.len;
    memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
    it2s_tender_unlock_trajectory(&facilities->epv);

    uint64_t now = it2s_tender_get_clock(&facilities->epv);

    trajectoryA[0].latitude = lat;
    trajectoryA[0].longitude = lon;
    trajectoryA[0].timestamp = now;
    ++trajectoryA_len;

    // Initiate conflict resolution
    pcm_req = calloc(1, sizeof(PCM_t));

    pcm_req->header.messageID = 43;
    pcm_req->header.protocolVersion = 1;
    pcm_req->header.stationID = 1;
    pthread_mutex_lock(&facilities->id.lock);
    pcm_req->header.stationID = facilities->id.station_id;
    pthread_mutex_unlock(&facilities->id.lock);
            
    pcm_req->pcm.currentPosition.latitude = lat;
    pcm_req->pcm.currentPosition.longitude = lon;
    asn_ulong2INTEGER(&pcm_req->pcm.currentPosition.timestamp, now);

    pcm_req->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
    ManeuverVehicleContainer_t* mvc = &pcm_req->pcm.maneuverContainer.choice.vehicle;

    // Vehicle Dimensions
    mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
    mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
    mvc->vehicleWidth = facilities->vehicle.width;

    // Planned trajectory
    mvc->plannedTrajectory.list.count = --trajectoryA_len;
    mvc->plannedTrajectory.list.size = trajectoryA_len * sizeof(void*);
    mvc->plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*));
    for (int i = 0; i < trajectoryA_len; ++i) {
        mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
        mvc->plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude;
        mvc->plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude;
        asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp);
    }

    // Desired trajectory
    mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
    mvc->negotiation->present = CoordinationNegotiation_PR_request;
    mvc->negotiation->choice.request.desiredTrajectories.list.count = 1;
    mvc->negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*);
    mvc->negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*));
    mvc->negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
    ProposedTrajectory_t* pt = mvc->negotiation->choice.request.desiredTrajectories.list.array[0];

    pt->trajectory.list.count = trajectoryA_len;
    pt->trajectory.list.size = sizeof(void*) * trajectoryA_len;
    pt->trajectory.list.array = malloc(sizeof(void*) * trajectoryA_len);
    for (int i = 0; i < trajectoryA_len; ++i) {
        pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
        pt->trajectory.list.array[i]->latitude = trajectoryA[i+1].latitude;
        pt->trajectory.list.array[i]->longitude = trajectoryA[i+1].longitude;
        asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp);
    }

    pt->offer = 5;
    pt->priority = 1;
    pt->id = 0;

    asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_PCM, pcm_req, buf, buf_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [pc] PCM.request encode failure (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }

    tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;
    tr->choice.packet.present = TransportPacketRequest_PR_btp;
    BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;

    bpr->btpType = BTPType_btpB;

    bpr->gn.destinationAddress.buf = malloc(6);
    for (int i = 0; i < 6; ++i) {
        bpr->gn.destinationAddress.buf[i] = 0xff;
    }
    bpr->gn.destinationAddress.size = 6;

    bpr->gn.packetTransportType = PacketTransportType_shb;

    bpr->destinationPort = 2043;

    bpr->gn.trafficClass = 2;

    bpr->data.buf = malloc(512);
    memcpy(bpr->data.buf, buf, buf_len);
    bpr->data.size = (enc.encoded + 7) / 8;

    buf[0] = 4;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [pc] TR PCM request encode failure");
        rv = 1;
        goto cleanup;
    }

    queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_PCM, pcm_req);
    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    return rv;
}

int pcm_check(void* fc, PCM_t* pcm) {
    facilities_t* facilities = (facilities_t*) fc;
    coordination_s* coordination = &facilities->coordination;

    int rv = 0;

    it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
    it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
    uint16_t trajectoryA_len = 0;
    uint16_t trajectoryB_len = 0;

    uint64_t now = it2s_tender_get_clock(&facilities->epv);

    int32_t lat, lon;
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    lat = facilities->epv.space.latitude;
    lon = facilities->epv.space.longitude;
    it2s_tender_unlock_space(&facilities->epv);

    pthread_mutex_lock(&coordination->lock);

    // Find neighbours
    int ni = -1;
    for (int i = 0; i < coordination->neighbours_len; ++i) {
        if (coordination->neighbours->station_id == pcm->header.stationID) {
            ni = i;
            break;
        }
    }

    // Add it if not found
    if (ni == -1 && coordination->neighbours_len + 1 < MC_MAX_NEIGHBOURS) {
        coordination->neighbours[coordination->neighbours_len].station_id = pcm->header.stationID;
        ni = coordination->neighbours_len;
        ++coordination->neighbours_len;
    }

    switch (pcm->pcm.maneuverContainer.present) {
        case ManeuverContainer_PR_vehicle:
            ;
            ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle;
            if (mvc->negotiation) {
                switch (mvc->negotiation->present) {
                    case CoordinationNegotiation_PR_request:
                        pcm_check_handle_request(facilities, pcm);
                        break; 
                    case CoordinationNegotiation_PR_reply:
                        pcm_check_handle_reply(facilities, pcm);
                        break; 
                    default:
                        break;
                }
            } else { 

                it2s_tender_lock_trajectory(&facilities->epv);
                trajectoryA_len = facilities->epv.trajectory.len;
                memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
                it2s_tender_unlock_trajectory(&facilities->epv);

                trajectoryA[0].latitude = lat;
                trajectoryA[0].longitude = lon;
                trajectoryA[0].timestamp = now;
                ++trajectoryA_len;

                trajectoryB[0].latitude = pcm->pcm.currentPosition.latitude;
                trajectoryB[0].longitude = pcm->pcm.currentPosition.longitude;
                asn_INTEGER2ulong(&pcm->pcm.currentPosition.timestamp, &trajectoryB[0].timestamp);
                ++trajectoryB_len;
                for (int i = 0; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
                    trajectoryB[i+1].latitude = mvc->plannedTrajectory.list.array[i]->latitude;
                    trajectoryB[i+1].longitude = mvc->plannedTrajectory.list.array[i]->longitude;
                    asn_INTEGER2ulong(&mvc->plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp);
                    ++trajectoryB_len;
                }

                bool intersecting = false;
                int index = -1;

                if (trajectoryA_len > 1 && trajectoryB_len > 1) {
                    intersecting = are_vehicles_intersecting(
                            trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width, 
                            trajectoryB, trajectoryB_len, mvc->vehicleLength.vehicleLengthValue, mvc->vehicleWidth,
                            &index);
                }

                if (intersecting) {
                    syslog_info("[facilities] [pc] intersection danger with %ld @ (%d, %d) in %ld ms", 
                            pcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
                    coordination->neighbours[ni].intersecting = true;
                    coordination->neighbours[ni].t_iid = now;
                    pcm_check_intersection_detected(facilities, pcm);
                }
            }
            break;
        case ManeuverContainer_PR_rsu:
            break;
        default:
            syslog_debug("[facilities] [pc] received PCM contains unrecognized ManeuverContainer type");
            rv = 1;
    }

    pthread_mutex_unlock(&coordination->lock);

    return rv;
}

static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_uper_len) {
    int rv = 0;

    it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN];

    PCM_t* pcm = calloc(1, sizeof(PCM_t));

    pcm->header.messageID = 43;
    pcm->header.protocolVersion = 1;
    uint64_t now = it2s_tender_get_clock(&facilities->epv);
    pthread_mutex_lock(&facilities->id.lock);
    pcm->header.stationID = facilities->id.station_id;
    pthread_mutex_unlock(&facilities->id.lock);

    int32_t lat, lon;
    uint16_t trajectory_len = 0;
    it2s_tender_lock_space(&facilities->epv);
    it2s_tender_get_space(&facilities->epv);
    lat = facilities->epv.space.latitude;
    lon = facilities->epv.space.longitude;
    it2s_tender_unlock_space(&facilities->epv);

    it2s_tender_lock_trajectory(&facilities->epv);
    trajectory_len = facilities->epv.trajectory.len;
    memcpy(trajectory, facilities->epv.trajectory.path, trajectory_len * sizeof(it2s_tender_st_s));
    it2s_tender_unlock_trajectory(&facilities->epv);

    pcm->pcm.currentPosition.latitude = lat;
    pcm->pcm.currentPosition.longitude = lon;
    asn_ulong2INTEGER(&pcm->pcm.currentPosition.timestamp, now);

    if (facilities->station_type == 15) {
        pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_rsu;
        ManeuverRSUContainer_t* mrc = &pcm->pcm.maneuverContainer.choice.rsu;
        mrc->recommendedTrajectories = NULL; // TODO
    } else {
        pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
        ManeuverVehicleContainer_t* mvc = &pcm->pcm.maneuverContainer.choice.vehicle;
        // Vehicle Dimensions
        mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
        mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
        mvc->vehicleWidth = facilities->vehicle.width;

        mvc->plannedTrajectory.list.count = trajectory_len;
        mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*);
        mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
        for (int i = 0; i < trajectory_len; ++i) {
            mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
            mvc->plannedTrajectory.list.array[i]->latitude = trajectory[i].latitude;
            mvc->plannedTrajectory.list.array[i]->longitude = trajectory[i].longitude;
            asn_ulong2INTEGER(&mvc->plannedTrajectory.list.array[i]->timestamp, trajectory[i].timestamp);
        }
    }

    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_PCM, NULL, pcm, pcm_uper, 512);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [pc] failed encoding PCM (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup; 
    }
    *pcm_uper_len = (enc.encoded + 7) / 8;

cleanup:
    ASN_STRUCT_FREE(asn_DEF_PCM, pcm);

    return rv;
}

void* pc_service(void* fc) {

    facilities_t* facilities = (facilities_t*) fc;
    coordination_s* coordination = (coordination_s*) &facilities->coordination;

    uint8_t pcm[512];

    TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;
    tr->choice.packet.present = TransportPacketRequest_PR_btp;
    BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;

    bpr->btpType = BTPType_btpB;

    bpr->gn.destinationAddress.buf = malloc(6);
    for (int i = 0; i < 6; ++i) {
        bpr->gn.destinationAddress.buf[i] = 0xff;
    }
    bpr->gn.destinationAddress.size = 6;

    bpr->gn.packetTransportType = PacketTransportType_shb;

    bpr->destinationPort = 2043;

    bpr->gn.trafficClass = 2;

    bpr->data.buf = malloc(512);

    uint8_t tr_oer[1024];
    tr_oer[0] = 4;

    int rv;

    while (!facilities->exit) {

        rv = mk_pcm(facilities, bpr->data.buf, (uint16_t *) &bpr->data.size);
        if (rv) {
            continue;
        }

        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023);
        if (enc.encoded == -1) {
            syslog_err("[facilities] encoding TR for PCM failed");
            continue;
        }

        queue_send(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
        
        pthread_mutex_lock(&coordination->lock);

        pthread_mutex_unlock(&coordination->lock);


        usleep(SLEEP_TIME_MS * 1000);
    }


    return NULL;
}

void coordination_init(coordination_s* coordination) {
    pthread_mutex_init(&coordination->lock, NULL);
}