#include "cam.h"
#include "facilities.h"

#include <it2s-asn/itss-transport/TransportRequest.h>
#include <it2s-asn/itss-facilities/FacilitiesIndication.h>
#include <it2s-asn/itss-management/ManagementRequest.h>
#include <it2s-asn/camv2/CAM.h>

#include <sys/mman.h>
#include <stdint.h>
#include <time.h>
#include <zmq.h>
#include <unistd.h>
#include <math.h>
#include <fcntl.h>

#include <it2s-tender/space.h>
#include <it2s-tender/time.h>
#include <it2s-tender/database.h>
#include <it2s-tender/constants.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>

#define EARTH_RADIUS 6369000.0
#define RAD_PER_DEG M_PI/180.0

// Using Vanetza's Path History constraints
#define PATH_HISTORY_ALLOWABLE_ERROR 0.47
#define PATH_HISTORY_CHORD_LENGTH_THRESHOLD 22.5
#define PATH_HISTORY_SMALL_DELTA_PHI 1.0
#define PATH_HISTORY_DISTANCE 200.0
#define PATH_HISTORY_MAX_ESTIMATED_RADIUS EARTH_RADIUS

const cid_ssp_bm_t CID_SSP_BM_MAP[] = {
    {"CenDsrcTollingZone/ProtectedCommunicationZonesRSU",                            0x8000},
    {"publicTransport/publicTransportContainer",                                     0x4000},
    {"specialTransport/specialTransportContainer",                                   0x2000},
    {"dangerousGoods/dangerousGoodsContainer",                                       0x1000},
    {"roadwork/roadWorksContainerBasic",                                             0x0800},
    {"rescue/rescueContainer",                                                       0x0400},
    {"emergency/emergencyContainer",                                                 0x0200},
    {"safetyCar/safetyCarContainer",                                                 0x0100},
    {"closedLanes/RoadworksContainerBasic",                                          0x0080},
    {"requestForRightOfWay/EmergencyContainer: EmergencyPriority",                   0x0040},
    {"requestForFreeCrossingAtATrafficLight/EmergencyContainer: EmergencyPriority",  0x0020},
    {"noPassing/SafetyCarContainer: TrafficRule",                                    0x0010},
    {"noPassingForTrucks/SafetyCarContainer: TrafficRule",                           0x0008},
    {"speedLimit/SafetyCarContainer",                                                0x0004},
    {"reserved0",                                                                    0x0002},
    {"reserved1",                                                                    0x0001},
};

static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_len) {
    /* CAM SSP scheme
     *
     * byte | description
     * ---------------------------------
     * 0    | SSP version control
     * 1-2  | Service-specific parameter
     * 3-30 | Reserved
     */

    if (permissions_len < 3) {
        log_debug("[ca] permissions length too small");
        return 0;
    }

    uint16_t perms_int = *(uint16_t*)(permissions+1);

    uint16_t perm_val = CID_SSP_BM_MAP[cid].bitmap_val;
    perm_val = (perm_val>>8) | (perm_val<<8);

    if ((perm_val & perms_int) == perm_val) return 1;
    else return 0;

}

/*
 * Implementation of Path History as per the report,
 *      "Vehicle Safety Communications – Applications (VSC-A) Final Report: Appendix Volume 1"
 *      "Appendix B-2 - Method One (Section 3.2)"
*/ 
static void path_history_update(pos_point_t* pn) {

    lightship_t* ls = &facilities.lightship;

    ++ls->concise_points_len;
    if (ls->concise_points_len > 3) {
        ls->concise_points_len = 3;
    }
    for (int i = ls->concise_points_len - (1 + ls->concise_keep_start); i > 0; --i) {
        ls->concise_points[i] = ls->concise_points[i-1];
    }
    ls->concise_points[0] = *pn;

    if (ls->concise_points_len == 3) {
        double actual_error;
        double actual_chord_length = itss_haversine(
                ls->concise_points[2].lat / 1.0e7,
                ls->concise_points[2].lon / 1.0e7,
                ls->concise_points[0].lat / 1.0e7,
                ls->concise_points[0].lon / 1.0e7
                );
        if (actual_chord_length > PATH_HISTORY_CHORD_LENGTH_THRESHOLD) {
            actual_error = PATH_HISTORY_ALLOWABLE_ERROR + 1.0;
        } else {
            double dtheta = ((int)ls->concise_points[0].heading - ls->concise_points[2].heading) / 10.0;
            double estimated_r;
            if (dtheta < PATH_HISTORY_SMALL_DELTA_PHI) {
                actual_error = 0;
                estimated_r = PATH_HISTORY_MAX_ESTIMATED_RADIUS;
            } else {
                estimated_r = actual_chord_length/(2*sin(RAD_PER_DEG * dtheta/2));
                double d = estimated_r * cos(RAD_PER_DEG * dtheta/2);
                actual_error = estimated_r - d;
            }

        }
        if (actual_error > PATH_HISTORY_ALLOWABLE_ERROR) { /* add to path history */
            if (ls->path_history_len != PATH_HISTORY_MAX_LEN) {
                ++ls->path_history_len;
            } else { /* Path History is full */
                free(ls->path_history[PATH_HISTORY_MAX_LEN - 1]);
            }

            for (int i = ls->path_history_len-1; i > 0; --i) {
                ls->path_history[i] = ls->path_history[i-1];
            }
            ls->path_history[0] = malloc(sizeof(pos_point_t));
            *ls->path_history[0] = ls->concise_points[1];

            ls->concise_keep_start = false;
        } else {
            ls->concise_keep_start = true;
        }
    }

    if (ls->path_history_len >= 2) {
        double distance = 0.0;
        for (int i = 0; i < ls->path_history_len - 1; ++i) {
            distance += itss_haversine(
                    ls->path_history[i]->lat / 1.0e7,
                    ls->path_history[i]->lon / 1.0e7,
                    ls->path_history[i+1]->lat / 1.0e7,
                    ls->path_history[i+1]->lon / 1.0e7
                    );
            if (distance > PATH_HISTORY_DISTANCE) {
                for (int j = i; j < ls->path_history_len - 1; ++j) {
                    free(ls->path_history[j+1]);
                }
                ls->path_history_len = i + 1;
                break;
            }
        }
    }
}

static int mk_cam(uint8_t *cam_oer, uint16_t *cam_len) {
    int rv = 0;
    int shm_fd, shm_valid = 0;

    lightship_t* lightship = &facilities.lightship;

    CAM_t *cam = calloc(1, sizeof(CAM_t));

    cam->header.protocolVersion = 2;
    cam->header.messageID = messageID_cam;
    pthread_mutex_lock(&facilities.id.lock);
    cam->header.stationID = facilities.id.station_id;
    pthread_mutex_unlock(&facilities.id.lock);
    cam->cam.camParameters.basicContainer.stationType = facilities.station_type;

    BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;

    uint64_t now = itss_time_get();

    pthread_mutex_lock(&lightship->lock);

    if (facilities.station_type != StationType_roadSideUnit) {
        cam->cam.generationDeltaTime = now % 65536;

        itss_space_lock();
        itss_space_get();
        bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
        bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;

        // Set GPS coordinates
        bc->referencePosition.latitude = epv.space.latitude;
        bc->referencePosition.longitude = epv.space.longitude;
        uint16_t lat_conf = epv.space.latitude_conf;
        uint16_t lon_conf = epv.space.longitude_conf;

        cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
        BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;

        // Set speed
        bvc_hf->speed.speedValue = epv.space.speed;
        bvc_hf->speed.speedConfidence = epv.space.speed_conf;

        // Set heading
        bvc_hf->heading.headingValue = epv.space.heading;
        bvc_hf->heading.headingConfidence = epv.space.heading_conf;

        itss_space_unlock(epv);

        if (lat_conf > lon_conf) {
            bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
            bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
            bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
        } else {
            bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
            bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
            bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
        }

        bvc_hf->vehicleWidth = facilities.vehicle.width;
        bvc_hf->vehicleLength.vehicleLengthValue = facilities.vehicle.length;
        bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
        bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
        bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;

        bvc_hf->driveDirection = DriveDirection_unavailable;
        bvc_hf->curvature.curvatureValue = CurvatureValue_unavailable;
        bvc_hf->curvature.curvatureConfidence = CurvatureConfidence_unavailable;
        bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable;
        bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable;

        /*
        // Save current values
        if (lightship->path_history_len == POS_HISTORY_MAX_LEN) {
            free(lightship->path_history[POS_HISTORY_MAX_LEN-1]); 
            --lightship->path_history_len;
        }
        */

        /*
        for (int i = lightship->path_history_len-1; i >= 0; --i) {
            lightship->path_history[i+1] = lightship->path_history[i];
        }
        */

        pos_point_t pn;
        pn.heading = bvc_hf->heading.headingValue;
        pn.lat = bc->referencePosition.latitude;
        pn.lon = bc->referencePosition.longitude;
        pn.alt = bc->referencePosition.altitude.altitudeValue;
        pn.speed = bvc_hf->speed.speedValue;
        pn.ts = now;
        path_history_update(&pn);

        lightship->t_last_cam = now;

        // Acceleration
        if (lightship->last_pos.ts != 0) {
            double delta_angle = ((int)pn.heading - lightship->last_pos.heading) / 10.0; /* 1º */
            double delta_speed = (pn.speed - (lightship->last_pos.speed * cos(delta_angle * RAD_PER_DEG))) / 10.0; /* 0.1 m/s */
            int16_t long_a =  (int16_t) (delta_speed / (double) ((pn.ts - lightship->last_pos.ts) / 1000.0)); /* 0.1 m/s^2 */

            if (long_a > 160) long_a = 160;
            else if (long_a < -160) long_a = -160;

            bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = long_a;
            bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
        } else {
            bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
            bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
        }

        // Low frequency container
        if (now > lightship->t_last_cam_lfc + 500) {
            cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(LowFrequencyContainer_t));

            cam->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;

            BasicVehicleContainerLowFrequency_t* bvc_lf = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency;

            PathHistory_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory;

            if (lightship->path_history_len != 0) {
                ph->list.array = malloc((lightship->path_history_len) * sizeof(void*));
                ph->list.count = lightship->path_history_len;
                ph->list.size = (lightship->path_history_len) * sizeof(void*);

                ph->list.array[0] = calloc(1,sizeof(PathPoint_t));

                if (lightship->path_history[0]->alt != AltitudeValue_unavailable && pn.alt != AltitudeValue_unavailable) {
                    ph->list.array[0]->pathPosition.deltaAltitude = lightship->path_history[0]->alt - pn.alt;
                } else {
                    ph->list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
                }

                if (lightship->path_history[0]->lat != Latitude_unavailable && pn.lat != Latitude_unavailable) {
                    ph->list.array[0]->pathPosition.deltaLatitude = lightship->path_history[0]->lat - pn.lat;
                } else {
                    ph->list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
                }

                if (lightship->path_history[0]->lon != Longitude_unavailable && pn.lon != Longitude_unavailable) {
                    ph->list.array[0]->pathPosition.deltaLongitude = lightship->path_history[0]->lon - pn.lon;
                } else {
                    ph->list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
                }

                ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
                *ph->list.array[0]->pathDeltaTime = (pn.ts - lightship->path_history[0]->ts)/10;

                for (int i = 1; i < lightship->path_history_len; ++i) {
                    ph->list.array[i] = calloc(1,sizeof(PathPoint_t));

                    if (lightship->path_history[i]->alt != AltitudeValue_unavailable && lightship->path_history[i-1]->alt != AltitudeValue_unavailable) {
                        ph->list.array[i]->pathPosition.deltaAltitude = lightship->path_history[i]->alt - lightship->path_history[i-1]->alt;
                    } else {
                        ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
                    }

                    if (lightship->path_history[i]->lat != Latitude_unavailable && lightship->path_history[i-1]->lat != Latitude_unavailable) {
                        ph->list.array[i]->pathPosition.deltaLatitude = lightship->path_history[i]->lat - lightship->path_history[i-1]->lat;
                    } else {
                        ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
                    }

                    if (lightship->path_history[i]->lon != Longitude_unavailable && lightship->path_history[i-1]->lon != Longitude_unavailable) {
                        ph->list.array[i]->pathPosition.deltaLongitude = lightship->path_history[i]->lon - lightship->path_history[i-1]->lon;
                    } else {
                        ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
                    }

                    ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
                    *ph->list.array[i]->pathDeltaTime = (lightship->path_history[i-1]->ts - lightship->path_history[i]->ts)/10;
                }
            }

            lightship->t_last_cam_lfc = now;
        } 

        lightship->last_pos = pn;

    } else {
        cam->cam.generationDeltaTime = now % 65536;

        itss_space_lock();
        itss_space_get();
        bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
        bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;

        // Set GPS coordinates
        bc->referencePosition.latitude = epv.space.latitude;
        bc->referencePosition.longitude = epv.space.longitude;
        bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
        bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
        bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
        itss_space_unlock();

        cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;

        if (lightship->protected_zones.pz_len > 0) {
            cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t));
            ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
            pzs->list.count = lightship->protected_zones.pz_len;
            pzs->list.size = lightship->protected_zones.pz_len * sizeof(void*);
            pzs->list.array = malloc(lightship->protected_zones.pz_len * sizeof(void*));
            for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
                pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t));
                pzs->list.array[i]->protectedZoneLatitude = lightship->protected_zones.pz[i]->protectedZoneLatitude;
                pzs->list.array[i]->protectedZoneLongitude = lightship->protected_zones.pz[i]->protectedZoneLongitude;
                pzs->list.array[i]->protectedZoneType = lightship->protected_zones.pz[i]->protectedZoneType;

                if (lightship->protected_zones.pz[i]->expiryTime) {
                    pzs->list.array[i]->expiryTime->size = lightship->protected_zones.pz[i]->expiryTime->size;
                    pzs->list.array[i]->expiryTime->buf = malloc(lightship->protected_zones.pz[i]->expiryTime->size);
                    memcpy(pzs->list.array[i]->expiryTime->buf, lightship->protected_zones.pz[i]->expiryTime->buf, lightship->protected_zones.pz[i]->expiryTime->size);
                }
                if (lightship->protected_zones.pz[i]->protectedZoneID) {
                    pzs->list.array[i]->protectedZoneID = malloc(8);
                    *pzs->list.array[i]->protectedZoneID = *lightship->protected_zones.pz[i]->protectedZoneID;
                }
                if (lightship->protected_zones.pz[i]->protectedZoneRadius) {
                    pzs->list.array[i]->protectedZoneRadius = malloc(8);
                    *pzs->list.array[i]->protectedZoneRadius = *lightship->protected_zones.pz[i]->protectedZoneRadius;
                }
            }
        }
    }

    pthread_mutex_unlock(&lightship->lock);


    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam, cam_oer, 512);
    if (enc.encoded == -1) {
        log_error("[ca] failed encoding CAM (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup; 
    }
    *cam_len = (enc.encoded + 7) / 8;

cleanup:
    ASN_STRUCT_FREE(asn_DEF_CAM, cam);

    return rv;
}


int lightship_init() {

    lightship_t* lightship = &facilities.lightship;

    lightship->protected_zones.pz = calloc(256, sizeof(void*));
    pthread_mutex_init(&lightship->lock, NULL);

    return 0;
}


int lightship_check() {
    int rv = 0;

    lightship_t* lightship = &facilities.lightship;

    uint64_t now = itss_time_get();

    pthread_mutex_lock(&lightship->lock);

    if (lightship->type == StationType_roadSideUnit) { // RSU
        if (lightship->is_vehicle_near && now > lightship->next_cam_min) {
            rv = 1; 
        }
    } else { // Vehicle
        if (now > lightship->next_cam_max) {
            rv = 1;
        } else if (now > lightship->next_cam_min) {

            itss_space_lock();
            itss_space_get();

            // Check heading delta > 4º
            int diff = epv.space.heading - lightship->last_pos.heading;
            if (abs(diff) > 40) rv = 1;

            if (!rv) {
                // Check speed delta > 0.5 m/s
                diff = epv.space.speed - lightship->last_pos.speed;
                if (abs(diff) > 50) rv = 1;

                if (!rv) {
                    // Check position delta > 4 m
                    // TODO make an *accurate* distance calculator using GPS coords
                    int32_t avg_speed = (epv.space.speed + lightship->last_pos.speed)/2 / 100; /* cm/s to m/s */
                    uint64_t delta_time = (now - lightship->t_last_cam) / 1000; /* ms to s */
                    if (avg_speed * delta_time > 4) rv = 1;
                }
            }
            itss_space_unlock(epv);
        }


        // Remove expired PZs
        for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
            uint64_t expiry;
            if (lightship->protected_zones.pz[i]->expiryTime) {
                asn_INTEGER2ulong(lightship->protected_zones.pz[i]->expiryTime, (unsigned long long*) &expiry);
                if (now >= expiry) {
                    ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->protected_zones.pz[i]);
                    for (int j = i; j < lightship->protected_zones.pz_len - 1; ++j) {
                        lightship->protected_zones.pz[j] = lightship->protected_zones.pz[j+1];
                    }
                    --lightship->protected_zones.pz_len;
                }
            }
        }
    }


    pthread_mutex_unlock(&lightship->lock);

    return rv;
}

void lightship_reset_timer() {

    lightship_t* lightship = &facilities.lightship;

    uint64_t now = itss_time_get();

    pthread_mutex_lock(&lightship->lock);

    if (lightship->type != StationType_roadSideUnit) { // Vehicle
        lightship->next_cam_max = now + lightship->vehicle_gen_max;
        lightship->next_cam_min = now + lightship->vehicle_gen_min;
    } else { // RSU
        if (now > lightship->t_last_vehicle + lightship->rsu_vehicle_permanence) {
            lightship->is_vehicle_near = false;
        }
        lightship->next_cam_min = now + lightship->rsu_gen_min;
    }

    pthread_mutex_unlock(&lightship->lock);
}

enum CAM_CHECK_R check_cam(BTPPacketIndication_t *bpi, CAM_t* cam, uint8_t* ssp, uint32_t ssp_len) {
    int rv = 0;
    lightship_t* lightship = &facilities.lightship;

    uint64_t now = itss_time_get();

    // Check permissions
    if (ssp) {
        if (cam->cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_rsuContainerHighFrequency &&
                cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
            if (!permissions_check(CID_PROTECTED_ZONES, ssp, ssp_len)) {
                rv = CAM_BAD_PERMISSIONS; 
                log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PROTECTED_ZONES].container);
                return rv;
            }
        }
        if (cam->cam.camParameters.specialVehicleContainer) {
            switch (cam->cam.camParameters.specialVehicleContainer->present) {
                case SpecialVehicleContainer_PR_NOTHING:
                    break;
                case SpecialVehicleContainer_PR_publicTransportContainer:
                    if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp, ssp_len)) {
                        rv = CAM_BAD_PERMISSIONS; 
                        log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PUBLIC_TRANSPORT].container);
                        return rv;
                    }
                    break;
                case SpecialVehicleContainer_PR_specialTransportContainer:
                    if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp, ssp_len)) {
                        rv = CAM_BAD_PERMISSIONS; 
                        log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPECIAL_TRANSPORT].container);
                        return rv;
                    }
                    break;
                case SpecialVehicleContainer_PR_dangerousGoodsContainer:
                    if (!permissions_check(CID_DANGEROUS_GOODS, ssp, ssp_len)) {
                        rv = CAM_BAD_PERMISSIONS; 
                        log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_DANGEROUS_GOODS].container);
                        return rv;
                    }
                    break;
                case SpecialVehicleContainer_PR_roadWorksContainerBasic:
                    if (!permissions_check(CID_ROADWORK, ssp, ssp_len)) {
                        rv = CAM_BAD_PERMISSIONS; 
                        log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_ROADWORK].container);
                        return rv;
                    }
                    if (cam->cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) {
                        if (!permissions_check(CID_CLOSED_LANES, ssp, ssp_len)) {
                            rv = CAM_BAD_PERMISSIONS; 
                            log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_CLOSED_LANES].container);
                            return rv;
                        }
                    }
                    break;
                case SpecialVehicleContainer_PR_rescueContainer:
                    if (!permissions_check(CID_RESCUE, ssp, ssp_len)) {
                        rv = CAM_BAD_PERMISSIONS; 
                        log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_RESCUE].container);
                        return rv;
                    }
                    break;
                case SpecialVehicleContainer_PR_emergencyContainer:
                    if (!permissions_check(CID_EMERGENCY, ssp, ssp_len)) {
                        rv = CAM_BAD_PERMISSIONS; 
                        log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_EMERGENCY].container);
                        return rv;
                    }
                    if (cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority &&
                            cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) { 
                        // TODO verify bitmap
                        uint8_t bm = *cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf;
                        if (bm & 0x02) {
                            if (!permissions_check(CID_REQUEST_FOR_RIGHT_OF_WAY, ssp, ssp_len)) {
                                rv = CAM_BAD_PERMISSIONS; 
                                log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_RIGHT_OF_WAY].container);
                                return rv;
                            }
                        }
                        if (bm & 0x01) {
                            if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp, ssp_len)) {
                                rv = CAM_BAD_PERMISSIONS; 
                                log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT].container);
                                return rv;
                            }
                        }
                    }
                    break;
                case SpecialVehicleContainer_PR_safetyCarContainer:
                    if (!permissions_check(CID_SAFETY_CAR, ssp, ssp_len)) {
                        rv = CAM_BAD_PERMISSIONS; 
                        log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SAFETY_CAR].container);
                        return rv;
                    }
                    if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
                        switch (*cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
                            case TrafficRule_noPassing:
                                if (!permissions_check(CID_NO_PASSING, ssp, ssp_len)) {
                                    rv = CAM_BAD_PERMISSIONS; 
                                    log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING].container);
                                    return rv;
                                }
                                break;
                            case TrafficRule_noPassingForTrucks:
                                if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp, ssp_len)) {
                                    rv = CAM_BAD_PERMISSIONS; 
                                    log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING_FOR_TRUCKS].container);
                                    return rv;
                                }
                                break;
                            default:
                                break;
                        }
                    }

                    if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) {
                        if (!permissions_check(CID_SPEED_LIMIT, ssp, ssp_len)) {
                            rv = CAM_BAD_PERMISSIONS; 
                            log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPEED_LIMIT].container);
                            return rv;
                        }
                    }
                    break;
            }
        }
    }

    pthread_mutex_lock(&lightship->lock);
    if (lightship->type == StationType_roadSideUnit) {
        // Send CAMs if vehicles nearby
        if (bpi->stationType != StationType_roadSideUnit && bpi->isNeighbour) {
            lightship->t_last_vehicle = now;
            lightship->is_vehicle_near = true;
        }
    } else {
        // Protected zones
        if (cam->cam.camParameters.basicContainer.stationType == StationType_roadSideUnit &&
                cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
            ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
            if (pzs->list.count > 0 && pzs->list.count + lightship->protected_zones.pz_len < 255) {

                bool new_pz = false;
                for (int k = 0; k < pzs->list.count; ++k) {

                    bool found = false;
                    for (int j = 0; j < lightship->protected_zones.pz_len; ++j) {
                        if (lightship->protected_zones.pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude &&
                                lightship->protected_zones.pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) {
                            found = true; 
                            break;
                        }
                    }
                    if (found) continue;

                    new_pz = true;

                    lightship->protected_zones.pz[lightship->protected_zones.pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t));
                    lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude;
                    lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude;
                    lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType;

                    if (pzs->list.array[k]->expiryTime) {
                        lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size;
                        lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size);
                        memcpy(lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size);
                    }
                    if (pzs->list.array[k]->protectedZoneID) {
                        lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = malloc(8);
                        *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID;
                    }
                    if (pzs->list.array[k]->protectedZoneRadius) {
                        lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = malloc(8);
                        *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius;
                    }
                    ++lightship->protected_zones.pz_len;
                }

                // Inform [management]
                if (new_pz) {
                    uint8_t b_oer[512];
                    ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
                    mreq->present = ManagementRequest_PR_attributes;
                    mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
                    mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct protectedZones));
                    mreq->choice.attributes.choice.set.protectedZones->list.count = lightship->protected_zones.pz_len;
                    mreq->choice.attributes.choice.set.protectedZones->list.size = lightship->protected_zones.pz_len * sizeof(void*);
                    mreq->choice.attributes.choice.set.protectedZones->list.array = calloc(lightship->protected_zones.pz_len, sizeof(void*));
                    for (int p = 0; p < lightship->protected_zones.pz_len; ++p) {
                        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512);
                        oer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &mreq->choice.attributes.choice.set.protectedZones->list.array[p], b_oer, enc.encoded);
                    }

                    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_oer, 512);
                    ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq);
                    void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
                    itss_0send(management_socket, b_oer, enc.encoded);
                    log_debug("[ca]-> sending MReq.attributes.set.protectedZones to ->[management]");
                    uint8_t code;
                    itss_0recv_rt(&management_socket, &code, 1, b_oer, enc.encoded, 1000);
                    itss_0close(management_socket);
                }
            }
        }
    }
    pthread_mutex_unlock(&lightship->lock);

    return rv;
}

static int check_pz() {

    lightship_t* lightship = &facilities.lightship;

    bool is_inside = false;

    itss_space_lock();
    itss_space_get();
    double lat = epv.space.latitude/10000000.0;
    double lon = epv.space.longitude/10000000.0;
    itss_space_unlock();

    pthread_mutex_lock(&lightship->lock);

    for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
        double d = itss_haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0);

        int pz_radius = 50;
        if (lightship->protected_zones.pz[i]->protectedZoneRadius) {
            pz_radius = *lightship->protected_zones.pz[i]->protectedZoneRadius;
        }
        if (d < pz_radius) {
            is_inside = true;
            break;
        } 
    }

    pthread_mutex_unlock(&lightship->lock);
    return is_inside;
}

void* ca_service() {
    int rv = 0;

    TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
    tr->present = TransportRequest_PR_packet;
    TransportPacketRequest_t* tpr = &tr->choice.packet;
    tpr->present = TransportPacketRequest_PR_btp;
    BTPPacketRequest_t *bpr = &tpr->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 = Port_cam;

    bpr->gn.trafficClass = 2;

    if (facilities.edm.enabled) {
        bpr->destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t));
        bpr->destinationPortInfo->size = 2;
        bpr->destinationPortInfo->buf = malloc(2);
        *(uint16_t*)bpr->destinationPortInfo->buf = 0xed;
    }

    const int buf_len = 1024;

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

    // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs

    bpr->gn.securityProfile.sign = true;

    FacilitiesIndication_t* fi = calloc(1,sizeof(FacilitiesIndication_t));
    fi->present = FacilitiesIndication_PR_message;
    FacilitiesMessageIndication_t* fmi = &fi->choice.message;
    fmi->itsMessageType = ItsMessageType_cam;
    fmi->data.buf = malloc(buf_len);

    uint8_t tr_oer[buf_len];
    uint8_t fi_oer[buf_len];
    tr_oer[0] = 4; // Facilities
    fi_oer[0] = 4; 
    while (!facilities.exit) {
        usleep(1000*50);

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

            if (facilities.edm.enabled) {
                edm_encap(bpr->data.buf, (uint16_t*) &bpr->data.size, buf_len, Port_cam);
            }

            memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size);
            fmi->data.size = bpr->data.size;

            // Check if inside PZ
            bpr->gn.communicationProfile = 0;
            if (facilities.station_type != 15 && check_pz()) bpr->gn.communicationProfile = 1;

            uint32_t id = itss_id(bpr->data.buf, bpr->data.size);
            bpr->id = id;
            fmi->id = id;

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

            asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 1023);
            if(enc_fdi.encoded == -1){
                log_error("encoding FI for cam failed");
                continue;
            }

            itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded+1, ITSS_TRANSPORT, id, "TR.packet.btp");

            itss_queue_send(facilities.tx_queue, fi_oer, enc_fdi.encoded+1, ITSS_APPLICATIONS, id, "FI.message");

            lightship_reset_timer();
            
            // Logging
            if (facilities.logging.dbms) {
                pthread_mutex_lock(&facilities.id.lock);
                uint32_t station_id = facilities.id.station_id;
                pthread_mutex_unlock(&facilities.id.lock);
                itss_db_add(facilities.logging.dbms, station_id, bpr->id, true, messageID_cam, NULL, bpr->data.buf, bpr->data.size);
            }

            if (facilities.logging.recorder) {
                uint16_t buffer_len = 2048;
                uint8_t buffer[buffer_len];
                int e = itss_management_record_packet_sdu(
                        buffer,
                        buffer_len,
                        bpr->data.buf,
                        bpr->data.size,
                        id,
                        itss_time_get(),
                        ITSS_FACILITIES,
                        true);
                if (e != -1) {
                    itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set");  
                }
            }
        }

    }

    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);

    return NULL;
}