#include "mcm.h"
#include "facilities.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <stdint.h>

#include <it2s-gnss.h>
#include <it2s-tender/epv.h>
#include <it2s-tender/space.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
#include <it2s-asn/etsi-its-v2/mcm/EI2_MCM.h>

static void mcm_forward_to_app(EI2_MCM_t* mcm){
    const uint16_t buf_len = 4096;
    uint8_t buf[buf_len];
    EIS_FacilitiesIndication_t *fi = NULL;

    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI2_MCM, NULL, mcm, buf, buf_len);
    if (enc.encoded == -1){
        log_error("[mc] MCM encode failure (%s)", enc.failed_type->name);
        goto cleanup;
    }
    uint16_t mcm_len = (enc.encoded + 7) / 8;

    fi = calloc(1, sizeof(EIS_FacilitiesIndication_t));
    fi->present = EIS_FacilitiesIndication_PR_message;
    fi->choice.message.id = itss_id(buf, mcm_len);
    fi->choice.message.itsMessageType = EIS_Port_mcm;
    fi->choice.message.data.size = mcm_len;
    fi->choice.message.data.buf = malloc(mcm_len);
    memcpy(fi->choice.message.data.buf, buf, mcm_len);
    buf[0] = ITSS_FACILITIES;
    enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_EIS_FacilitiesIndication, fi, buf + 1, buf_len - 1);
    if (enc.encoded == -1){
        log_error("[mc] TR MCM.reply encode failure (%s)", enc.failed_type->name);
        goto cleanup;
    }

    itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buf, enc.encoded + 1, ITSS_APPLICATIONS, itss_id(buf, mcm_len), "FI.message"));

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
}

static int mk_mcm(){ 
    uint64_t gdt, traj_len, lat, lon, alt, speed, heading, lat_conf, lon_conf, alt_conf, speed_conf, heading_conf;
    struct GNSSDeltaPosition traj_points[16];
    int rv = 0;
    
    itss_time_lock();
    gdt = itss_time_get() % 65536;
    itss_time_unlock();

    itss_space_lock();
    itss_space_get();
    lat = epv.space.data.latitude.value;
    lat_conf = epv.space.data.latitude.confidence;
    lon = epv.space.data.longitude.value;
    lon_conf = epv.space.data.longitude.confidence;
    alt = epv.space.data.altitude.value;
    alt_conf = epv.space.data.altitude.confidence;
    speed = epv.space.data.speed.value;
    speed_conf = epv.space.data.speed.confidence;
    heading = epv.space.data.heading.value;
    heading_conf = epv.space.data.heading.confidence;
    traj_len = epv.space.data.ptraj_len;
    memcpy(traj_points, &epv.space.data.ptraj, sizeof(struct GNSSDeltaPosition)*traj_len);
    itss_space_unlock();

    // MCM
    EI2_MCM_t* mcm = calloc(1, sizeof(EI2_MCM_t));

    // Header
    EI2_ItsPduHeader_t* header = &mcm->header;
    header->protocolVersion = 2;
    header->messageId = EI2_MessageId_mcm; 
    header->stationId = facilities.id.station_id;

    // Payload
    EI2_WrappedMcmInformationBlocks_t* payload = &mcm->payload;

    EI2_McmBasicContainer_t* basicContainer = &payload->basicContainer;
    asn_uint642INTEGER(&basicContainer->generationDeltaTime, gdt);
    basicContainer->stationType = facilities.station_type;
    basicContainer->referencePosition.latitude = lat;
    basicContainer->referencePosition.longitude = lon;
    basicContainer->referencePosition.altitude.altitudeValue = alt;
    basicContainer->referencePosition.altitude.altitudeConfidence = alt_conf;
    if (lat_conf > lon_conf) {
        basicContainer->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lon_conf;
        basicContainer->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lat_conf;
        basicContainer->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
    } 
    else {
        basicContainer->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lon_conf;
        basicContainer->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lat_conf;
        basicContainer->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
    }

    EI2_McmContainer_t* mcmContainer = &payload->mcmContainer;
    mcmContainer->present = EI2_McmContainer_PR_vehiclemaneuverContainer;
    EI2_VehiclemaneuverContainer_t* vmcContainer = &mcmContainer->choice.vehiclemaneuverContainer;
    vmcContainer->mcmType = EI2_McmType_intent;
    vmcContainer->maneuverId = 0;
    vmcContainer->maneuverCoordinationConcept = EI2_ManeuverCoordinationConcept_agreementSeeking;
    vmcContainer->maneuverCoordinationRational.present = EI2_ManeuverCoordinationRational_PR_maneuverCooperationGoal;
    vmcContainer->maneuverCoordinationRational.choice.maneuverCooperationGoal = EI2_ManeuverCooperationGoal_vehicleInterception;
    vmcContainer->maneuverExecutionStatus = EI2_ManeuverExecutionStatus_started;
    vmcContainer->trajectoryId = 0;
    vmcContainer->vehicleTrajectory.present = EI2_vehicleTrajectory_PR_wgs84Trajectory;

    EI2_VehicleCurrentState_t* vcs = &vmcContainer->vehicleCurrentState;
    EI2_McmGenericCurrentState_t* mgcs = &vcs->mcmGenericCurrentState;
    mgcs->mcmType = EI2_McmType_intent;
    mgcs->maneuverId = 0;
    mgcs->maneuverCoordinationConcept = EI2_ManeuverCoordinationConcept_agreementSeeking;
    mgcs->maneuverCoordinationRational.present = EI2_ManeuverCoordinationRational_PR_maneuverCooperationGoal;
    mgcs->maneuverCoordinationRational.choice.maneuverCooperationGoal = EI2_ManeuverCooperationGoal_vehicleInterception;
    mgcs->maneuverExecutionStatus = EI2_ManeuverExecutionStatus_started;
    EI2_VehicleAutomationState_t* vas = &vcs->vehicleAutomationState;
    vas->humanDrivingLongitudinalAutomated = false;
    vas->humanDrivenLateralAutomated = false;
    vas->automatedDriving = false;
    vcs->speed.speedValue = speed;
    vcs->speed.speedConfidence = speed_conf;
    vcs->heading.headingValue = heading;
    vcs->heading.headingConfidence = heading_conf;
    vcs->longitudinalAcceleration.longitudinalAccelerationValue = EI2_AccelerationValue_unavailable;
    vcs->longitudinalAcceleration.longitudinalAccelerationConfidence = EI2_AccelerationConfidence_unavailable;
    vcs->vehicleSize.vehicleLenth = 0;
    vcs->vehicleSize.vehicleWidth = 0;

    if (traj_len > 10)
        traj_len = 10;
    EI2_Wgs84Trajectory_t* trajectory = &vmcContainer->vehicleTrajectory.choice.wgs84Trajectory;
    trajectory->trajectoryPoints.list.count = traj_len;
    trajectory->trajectoryPoints.list.size = sizeof(EI2_Wgs84TrajectoryPoint_t*);
    trajectory->trajectoryPoints.list.array = calloc(trajectory->trajectoryPoints.list.count, trajectory->trajectoryPoints.list.size);

    for (int i = 0; i < trajectory->trajectoryPoints.list.count; i++){
        trajectory->trajectoryPoints.list.array[i] = calloc(1, sizeof(EI2_Wgs84TrajectoryPoint_t));
        EI2_Wgs84TrajectoryPoint_t* trajectory_point = trajectory->trajectoryPoints.list.array[i];
        trajectory_point->intermediatePoint.present = EI2_IntermediatePoint_PR_reference;
        trajectory_point->longitudePosition = lon+traj_points[i].deltaLongitude._0;
        trajectory_point->latitudePosition = lat+traj_points[i].deltaLatitude._0;
        trajectory_point->speed.speedValue = EI2_SpeedValue_unavailable;
        trajectory_point->speed.speedConfidence = EI2_SpeedConfidence_unavailable;
        EI2_IntermediatePointReference_t* ipr = &trajectory_point->intermediatePoint.choice.reference;
        ipr->deltaReferencePosition.deltaLatitude = traj_points[i].deltaLongitude._0;
        ipr->deltaReferencePosition.deltaLongitude = traj_points[i].deltaLatitude._0;
        ipr->deltaReferencePosition.deltaAltitude = traj_points[i].deltaAltitude._0;
        ipr->referenceHeading.headingValue = EI2_HeadingValue_unavailable;
        ipr->referenceHeading.headingConfidence = EI2_HeadingConfidence_unavailable;
        ipr->lane.lanePosition = EI2_LanePosition_innerHardShoulder;
        ipr->lane.laneCount = 1;
        ipr->timeOfPos = traj_points[i].deltaTime._0;
    }

    mcm_forward_to_app(mcm);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EI2_MCM, mcm);

    return rv;
}

void *mc_service(){
    int rv;
    mcm_coord_t *mcm_coord = (mcm_coord_t *)&facilities.mcm_coord;

    while (!facilities.exit){
        sleep(1);
        
        pthread_mutex_lock(&mcm_coord->lock);
        rv = mk_mcm();
        if (rv)
            continue;
        pthread_mutex_unlock(&mcm_coord->lock);
    }

    return NULL;
}

void mcm_coord_init(){
    mcm_coord_t *coord = &facilities.mcm_coord;
    pthread_mutex_init(&coord->lock, NULL);
}