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

#include <itss-transport/BTPDataRequest.h>
#include <camv2/CAM.h>
#include <it2s-gps.h>

#include <stdint.h>
#include <time.h>
#include <zmq.h>
#include <unistd.h>
#include <syslog.h>
#include <math.h>

#define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__)
#define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]",  __func__, __LINE__, ##__VA_ARGS__)
#define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]",  __func__, __LINE__, ##__VA_ARGS__)

#ifndef NDEBUG
#define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__)
#else
#define syslog_debug(msg, ...)
#endif

#define LEAP_SECONDS 5

static AltitudeConfidence_t getAltitudeConfidence(double conf) {
    if (conf >= 200) return AltitudeConfidence_outOfRange;
    if (conf >= 100) return AltitudeConfidence_alt_200_00;
    if (conf >= 50)  return AltitudeConfidence_alt_100_00;
    if (conf >= 20)  return AltitudeConfidence_alt_050_00;
    if (conf >= 10)  return AltitudeConfidence_alt_020_00;
    if (conf >= 5)   return AltitudeConfidence_alt_010_00;
    if (conf >= 1)   return AltitudeConfidence_alt_005_00;

    return AltitudeConfidence_alt_001_00;
}

static HeadingConfidence_t getHeadingConfidence(uint32_t conf) {
    if (conf > 125)  return 126;
    if (conf == 125) return 125;
    if (conf == 0)   return 127;
    if (conf < 1)    return 1;

    return conf;
}

static SpeedConfidence_t getSpeedConfidence(uint32_t conf) {
    if (conf > 125)  return 126;
    if (conf == 125) return 125;
    if (conf == 0)   return 127;
    if (conf < 1)    return 1;

    return conf;
}


static int mk_cam(uint8_t *cam, uint32_t *cam_len) {
    int rv = 0;

    struct timespec st;
    if (clock_gettime(CLOCK_REALTIME, &st)) {
        syslog_emerg("clock_gettime() failed");
    }

    CAM_t *cam_tx = calloc(1, sizeof(CAM_t));
    
    cam_tx->header.protocolVersion = 2;
    cam_tx->header.messageID = ItsPduHeader__messageID_cam;
    cam_tx->header.stationID = 0;
    cam_tx->cam.camParameters.basicContainer.stationType = StationType_passengerCar;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;

    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureValue = CurvatureValue_unavailable;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureConfidence = CurvatureConfidence_unavailable;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable;
    cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable;

    cam_tx->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(struct LowFrequencyContainer));
    cam_tx->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.vehicleRole = VehicleRole_default;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf = calloc(1, sizeof(uint8_t) * 1);
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf[0] = 1 << 6;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.bits_unused = 5;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.size = 1;
    *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf) = 0x00;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 0;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 0;

    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 1;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 1;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array = calloc(1, sizeof(struct PathPoint*));
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0] = calloc(1, sizeof(struct PathPoint));
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime = calloc(1, sizeof(PathDeltaTime_t));
    *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime) = PathDeltaTime_tenMilliSecondsInPast;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
    cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;

    struct timespec systemtime;
    if (clock_gettime(CLOCK_REALTIME, &systemtime)) {
        syslog_emerg("clock_gettime() failed");
    }

    long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
    generationdeltatime = generationdeltatime - 1072915200000;  // EPOCH -> 2004/01/01 00:00:000
    generationdeltatime = generationdeltatime % 64536;
    cam_tx->cam.generationDeltaTime = generationdeltatime;

    struct it2s_gps_data gps_data;
    it2s_gps_read(&gps_data);
   if (!isnan(gps_data.gps_altitude)) {
       cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = (int32_t)((gps_data.gps_altitude) * 10);
       cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = getAltitudeConfidence(gps_data.gps_epv);
   } else {
     cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable;
     cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
   }

   if (!isnan(gps_data.gps_latitude)) {
       cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((gps_data.gps_latitude) * 10000000);
   } else {
     cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable;
   }

   if (!isnan(gps_data.gps_longitude)) {
       cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((gps_data.gps_longitude) * 10000000);
   } else {
     cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable;
   }

    cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
    cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
    cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;

    cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
    
    if (!isnan(gps_data.gps_track)) {
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10));
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10));
    } else {
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable;
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable;
    
    }

    if (!isnan(gps_data.gps_speed)) {
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100));  // cm/s
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data.gps_eps * 100));
    } else {
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable;
        cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable;
    }

    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256);
    if (enc.encoded == -1) {
        rv = 1;
        goto cleanup; 
    }
    *cam_len = (enc.encoded + 7) / 8;

cleanup:
    ASN_STRUCT_FREE(asn_DEF_CAM, cam_tx);

    return rv;
}


void *ca_service(void *fc) {

    int rv = 0;
    uint8_t code = 0;
    facilities_t *facilities = (facilities_t*) fc;

    void* h = zmq_socket(facilities->ctx, ZMQ_REQ);
    zmq_connect(h, TRANSPORT_ADDRESS);

    BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));

    bdr->btpType = BTPType_btpB;

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

    bdr->gnPacketTransportType = PacketTransportType_shb;

    bdr->destinationPort = Port_cam;

    bdr->gnTrafficClass = 2;

    bdr->data.buf = malloc(256);

    uint8_t bdr_oer[256];
    bdr_oer[0] = 4; // Facilities
    while (!facilities->exit) {
        rv = mk_cam(bdr->data.buf, (uint32_t *) &bdr->data.size);
        if (rv) {
            syslog_err("[facilities] make cam failed (%d)", rv);
            continue;
        }

        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 255);
        if (enc.encoded == -1) {
            syslog_err("[facilities] encoding BTPDataRequest for cam failed");
            continue;
        }

        queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
        pthread_cond_signal(&facilities->tx_queue->trigger);

        usleep(1000*1000);
    }

    ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);

    return NULL;
}