//
// Copyright 2015, Oliver Jowett <oliver@mutability.co.uk>
//

// This file is free software: you may copy, redistribute and/or modify it  
// under the terms of the GNU General Public License as published by the
// Free Software Foundation, either version 2 of the License, or (at your  
// option) any later version.  
//
// This file is distributed in the hope that it will be useful, but  
// WITHOUT ANY WARRANTY; without even the implied warranty of  
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU  
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License  
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "uat.h"
#include "uat_decode.h"

#define _UNUSED(V) ((void) V)

typedef enum { UAT_UPLINK, UAT_DOWNLINK } frame_type_t;

// Function pointer type for a handler called by dump978_read_frames().
// It is called with arguments:
//   t: frame type (UAT_UPLINK or UAT_DOWNLINK)
//   f: pointer to frame data buffer
//   l: length of frame data
//   d1: value of handler_data passed to handler
//   d2: value of handler_data passed to handler
//  ss: measured signal strength in dBFS
// The frame data buffer is a shared buffer owned by the caller
// and may be reused after return; if the handler needs to
// preserve the data after returning, it should take a copy.
typedef void (*frame_handler_t)(frame_type_t t,uint8_t *f,int l,char *d1, char *d2, float ss);

static char* checksum_and_print(uint8_t *frame, int len, uint32_t parity, char *p, char *end);

// If you call this with constants for firstbit/lastbit
// gcc will do a pretty good job of crunching it down
// to just a couple of operations. Even more so if value
// is also constant.
static inline void setbits(uint8_t *frame, unsigned firstbit, unsigned lastbit, uint32_t value)
{
    // convert to 0-based:
    unsigned lb = lastbit-1;

    // align value with byte layout:
    unsigned offset = 7 - (lb&7);
    unsigned nb = (lastbit - firstbit + 1 + offset);
    uint32_t mask = (1 << (lastbit-firstbit+1))-1;
    uint32_t imask = ~(mask << offset);
    uint32_t aligned = (value & mask) << offset;

    frame[lb >> 3] = (frame[lb >> 3] & imask) | aligned;
    if (nb > 8)
        frame[(lb >> 3) - 1] = (frame[(lb >> 3) - 1] & (imask >> 8)) | (aligned >> 8);
    if (nb > 16)
        frame[(lb >> 3) - 2] = (frame[(lb >> 3) - 2] & (imask >> 16)) | (aligned >> 16);
    if (nb > 24)
        frame[(lb >> 3) - 3] = (frame[(lb >> 3) - 3] & (imask >> 24)) | (aligned >> 24);
}

static int encode_altitude(int ft)
{
    int i;

    i = (ft + 1000) / 25;
    if (i < 0) i = 0;
    if (i > 0x7FF) i = 0x7FF;

    return (i & 0x000F) | 0x0010 | ((i & 0x07F0) << 1);
}

static int encode_ground_speed(int kt)
{
    if (kt > 175)
        return 124;
    if (kt > 100)
        return (kt - 100) / 5 + 108;
    if (kt > 70)
        return (kt - 70) / 2 + 93;
    if (kt > 15)
        return (kt - 15) + 38;
    if (kt > 2)
        return (kt - 2) * 2 + 11;
    if (kt == 2)
        return 12;
    if (kt == 1)
        return 8;
    return 1;
}

static int encode_air_speed(int kt, int supersonic)
{
    int sign;

    if (kt < 0) {
        sign = 0x0400;
        kt = -kt;
    } else {
        sign = 0;
    }

    if (supersonic)
        kt = kt / 4;

    ++kt;
    if (kt > 1023)
        kt = 1023;

    return kt | sign;
}

static int encode_vert_rate(int rate)
{
    int sign;

    if (rate < 0) {
        sign = 0x200;
        rate = -rate;
    } else {
        sign = 0;
    }

    rate = (rate / 64) + 1;
    if (rate > 511)
        rate = 511;

    return rate | sign;
}

static double cprMod(double a, double b) {
    double res = fmod(a, b);
    if (res < 0) res += b;
    return res;
}

static int cprNL(double lat)
{
    if (lat < 0) lat = -lat;
    if (lat < 10.47047130) return 59;
    if (lat < 14.82817437) return 58;
    if (lat < 18.18626357) return 57;
    if (lat < 21.02939493) return 56;
    if (lat < 23.54504487) return 55;
    if (lat < 25.82924707) return 54;
    if (lat < 27.93898710) return 53;
    if (lat < 29.91135686) return 52;
    if (lat < 31.77209708) return 51;
    if (lat < 33.53993436) return 50;
    if (lat < 35.22899598) return 49;
    if (lat < 36.85025108) return 48;
    if (lat < 38.41241892) return 47;
    if (lat < 39.92256684) return 46;
    if (lat < 41.38651832) return 45;
    if (lat < 42.80914012) return 44;
    if (lat < 44.19454951) return 43;
    if (lat < 45.54626723) return 42;
    if (lat < 46.86733252) return 41;
    if (lat < 48.16039128) return 40;
    if (lat < 49.42776439) return 39;
    if (lat < 50.67150166) return 38;
    if (lat < 51.89342469) return 37;
    if (lat < 53.09516153) return 36;
    if (lat < 54.27817472) return 35;
    if (lat < 55.44378444) return 34;
    if (lat < 56.59318756) return 33;
    if (lat < 57.72747354) return 32;
    if (lat < 58.84763776) return 31;
    if (lat < 59.95459277) return 30;
    if (lat < 61.04917774) return 29;
    if (lat < 62.13216659) return 28;
    if (lat < 63.20427479) return 27;
    if (lat < 64.26616523) return 26;
    if (lat < 65.31845310) return 25;
    if (lat < 66.36171008) return 24;
    if (lat < 67.39646774) return 23;
    if (lat < 68.42322022) return 22;
    if (lat < 69.44242631) return 21;
    if (lat < 70.45451075) return 20;
    if (lat < 71.45986473) return 19;
    if (lat < 72.45884545) return 18;
    if (lat < 73.45177442) return 17;
    if (lat < 74.43893416) return 16;
    if (lat < 75.42056257) return 15;
    if (lat < 76.39684391) return 14;
    if (lat < 77.36789461) return 13;
    if (lat < 78.33374083) return 12;
    if (lat < 79.29428225) return 11;
    if (lat < 80.24923213) return 10;
    if (lat < 81.19801349) return 9;
    if (lat < 82.13956981) return 8;
    if (lat < 83.07199445) return 7;
    if (lat < 83.99173563) return 6;
    if (lat < 84.89166191) return 5;
    if (lat < 85.75541621) return 4;
    if (lat < 86.53536998) return 3;
    if (lat < 87.00000000) return 2;
    else return 1;
}

static int cprN(double lat, int odd)
{
    int nl = cprNL(lat) - (odd ? 1 : 0);
    if (nl < 1)
        nl = 1;
    return nl;
}

static int encode_cpr_lat(double lat, double lon, int odd, int surface)
{
    _UNUSED(lon);
    int NbPow = (surface ? 1<<19 : 1<<17);

    double Dlat = 360.0 / (odd ? 59 : 60);
    int YZ = floor(NbPow * cprMod(lat, Dlat) / Dlat + 0.5);

    return YZ & 0x1FFFF; // always a 17-bit field
}

static int encode_cpr_lon(double lat, double lon, int odd, int surface)
{
    int NbPow = (surface ? 1<<19 : 1<<17);

    double Dlat = 360.0 / (odd ? 59 : 60);
    int YZ = floor(NbPow * cprMod(lat, Dlat) / Dlat + 0.5);

    double Rlat = Dlat * (1.0 * YZ / NbPow + floor(lat / Dlat));
    double Dlon = (360.0 / cprN(Rlat, odd));
    int XZ = floor(NbPow * cprMod(lon, Dlon) / Dlon + 0.5);

    return XZ & 0x1FFFF; // always a 17-bit field
}

static int encode_cf(struct uat_adsb_mdb *mdb)
{
    // Encode the CF field for DF 18; this says whether this
    // is a TIS-B message, an ADS-B rebroadcast, etc
    switch (mdb->address_qualifier) {
        case AQ_ADSB_ICAO:
            return 6; // ADS-B rebroadcast, will use IMF=0
        case AQ_TISB_ICAO:
            return 2; // Fine TIS-B message, will use IMF=0
        case AQ_TISB_OTHER:
            return 2; // Fine TIS-B message, will use IMF=1
        default:
            // national, fixed beacon, vehicle, reserved
            return 1; // ES/NT devices that use other addressing techniques
    }
}

static int encode_imf(struct uat_adsb_mdb *mdb)
{
    // Encode the IMF bit for DF 18; this is 0 if the address
    // is a regular 24-bit ICAO address, or 1 if it uses a
    // different format.
    switch (mdb->address_qualifier) {
        case AQ_ADSB_ICAO:
        case AQ_TISB_ICAO:
            return 0;

        default:
            return 1;
    }
}

static char* send_altitude_only(struct uat_adsb_mdb *mdb, char *p, char *end)
{
    uint8_t esnt_frame[14] = { 0 };
    int raw_alt;

    // Need barometric altitude, see if we have it
    if (mdb->altitude_type == ALT_BARO) {
        raw_alt = encode_altitude(mdb->altitude);
    } else if (mdb->sec_altitude_type == ALT_BARO) {
        raw_alt = encode_altitude(mdb->sec_altitude);
    } else {
        raw_alt = 0;
    }

    setbits(esnt_frame, 1, 5, 18);                 // DF=18, ES/NT
    setbits(esnt_frame, 6, 8, encode_cf(mdb));     // CF
    setbits(esnt_frame, 9, 32, mdb->address);      // AA

    // ES:
    setbits(esnt_frame+4, 1, 5, 0);                // FORMAT TYPE CODE = 0, barometric altitude with no position
    setbits(esnt_frame+4, 6, 7, 0);                // SURVEILLANCE STATUS normal
    setbits(esnt_frame+4, 8, 8, encode_imf(mdb));  // IMF
    setbits(esnt_frame+4, 9, 20, raw_alt);         // ALTITUDE
    setbits(esnt_frame+4, 21, 21, 0);              // TIME (T)
    setbits(esnt_frame+4, 22, 22, 0);              // CPR FORMAT (F)
    setbits(esnt_frame+4, 23, 39, 0);              // ENCODED LATITUDE
    setbits(esnt_frame+4, 40, 56, 0);              // ENCODED LONGITUDE

    p = checksum_and_print(esnt_frame, 14, 0, p, end);
    return p;
}

static char* maybe_send_surface_position(struct uat_adsb_mdb *mdb, char *p, char *end)
{
    uint8_t esnt_frame[14] = { 0 };

    if (mdb->airground_state != AG_GROUND)
        return p; // nope!

    setbits(esnt_frame, 1, 5, 18);                 // DF=18, ES/NT
    setbits(esnt_frame, 6, 8, encode_cf(mdb));     // CF
    setbits(esnt_frame, 9, 32, mdb->address);      // AA

    setbits(esnt_frame+4, 1, 5, 8);                                            // FORMAT TYPE CODE = 8, surface position (NUCp=6)

    if (!mdb->speed_valid) {
        setbits(esnt_frame+4, 6, 12, 0);                                       // MOVEMENT: invalid
    } else {
        setbits(esnt_frame+4, 6, 12, encode_ground_speed(mdb->speed));         // MOVEMENT
    }

    if (mdb->track_type != TT_TRACK) {
        setbits(esnt_frame+4, 13, 13, 0);                                      // STATUS for ground track: invalid
        setbits(esnt_frame+4, 14, 20, 0);                                      // GROUND TRACK (TRUE)
    } else {
        setbits(esnt_frame+4, 13, 13, 1);                                      // STATUS for ground track: valid
        setbits(esnt_frame+4, 14, 20, mdb->track * 128 / 360);                 // GROUND TRACK (TRUE)
    }

    setbits(esnt_frame+4, 21, 21, encode_imf(mdb));                            // IMF

    // even frame:
    setbits(esnt_frame+4, 22, 22, 0);                                          // CPR FORMAT (F) = even
    setbits(esnt_frame+4, 23, 39, encode_cpr_lat(mdb->lat, mdb->lon, 0, 1));   // ENCODED LATITUDE
    setbits(esnt_frame+4, 40, 56, encode_cpr_lon(mdb->lat, mdb->lon, 0, 1));   // ENCODED LONGITUDE
    p = checksum_and_print(esnt_frame, 14, 0, p, end);

    // odd frame:
    setbits(esnt_frame+4, 22, 22, 1);                                          // CPR FORMAT (F) = odd
    setbits(esnt_frame+4, 23, 39, encode_cpr_lat(mdb->lat, mdb->lon, 1, 1));   // ENCODED LATITUDE
    setbits(esnt_frame+4, 40, 56, encode_cpr_lon(mdb->lat, mdb->lon, 1, 1));   // ENCODED LONGITUDE
    p = checksum_and_print(esnt_frame, 14, 0, p, end);

    return p;
}

static char* maybe_send_air_position(struct uat_adsb_mdb *mdb, char *p, char *end)
{
    uint8_t esnt_frame[14] = { 0 };
    int raw_alt;

    if (mdb->airground_state != AG_SUPERSONIC && mdb->airground_state != AG_SUBSONIC)
        return p; // nope!

    if (!mdb->position_valid) {
        p = send_altitude_only(mdb, p, end);
        return p;
    }

    setbits(esnt_frame, 1, 5, 18);            // DF=18, ES/NT
    setbits(esnt_frame, 6, 8, encode_cf(mdb));// CF
    setbits(esnt_frame, 9, 32, mdb->address); // AA

    // decide on a metype
    switch (mdb->altitude_type) {
        case ALT_BARO:
            setbits(esnt_frame+4, 1, 5, 18);           // FORMAT TYPE CODE = 18, airborne position (baro alt)
            raw_alt = encode_altitude(mdb->altitude);
            break;

        case ALT_GEO:
            setbits(esnt_frame+4, 1, 5, 22);           // FORMAT TYPE CODE = 22, airborne position (GNSS alt)
            raw_alt = encode_altitude(mdb->altitude);
            break;

        default:
            setbits(esnt_frame+4, 1, 5, 18);           // FORMAT TYPE CODE = 18, airborne position (baro alt)
            raw_alt = 0; // unavailable
            break;
    }

    setbits(esnt_frame+4, 6, 7, 0);                // SURVEILLANCE STATUS normal
    setbits(esnt_frame+4, 8, 8, encode_imf(mdb));  // IMF
    setbits(esnt_frame+4, 9, 20, raw_alt);         // ALTITUDE
    setbits(esnt_frame+4, 21, 21, 0);              // TIME (T)

    // even frame:
    setbits(esnt_frame+4, 22, 22, 0);                                               // CPR FORMAT (F) - even
    setbits(esnt_frame+4, 23, 39, encode_cpr_lat(mdb->lat, mdb->lon, 0, 0));        // ENCODED LATITUDE
    setbits(esnt_frame+4, 40, 56, encode_cpr_lon(mdb->lat, mdb->lon, 0, 0));        // ENCODED LONGITUDE
    p = checksum_and_print(esnt_frame, 14, 0, p, end);

    // odd frame:
    setbits(esnt_frame+4, 22, 22, 1);                                               // CPR FORMAT (F) - odd
    setbits(esnt_frame+4, 23, 39, encode_cpr_lat(mdb->lat, mdb->lon, 1, 0));        // ENCODED LATITUDE
    setbits(esnt_frame+4, 40, 56, encode_cpr_lon(mdb->lat, mdb->lon, 1, 0));        // ENCODED LONGITUDE
    p = checksum_and_print(esnt_frame, 14, 0, p, end);

    return p;
}

static char* maybe_send_air_velocity(struct uat_adsb_mdb *mdb, char *p, char *end)
{
    uint8_t esnt_frame[14] = { 0 };
    int supersonic;

    if (mdb->airground_state != AG_SUPERSONIC && mdb->airground_state != AG_SUBSONIC)
        return p; // nope!

    if (!mdb->ew_vel_valid && !mdb->ns_vel_valid && mdb->vert_rate_source == ALT_INVALID) {
        // not really any point sending this
        return p;
    }

    setbits(esnt_frame, 1, 5, 18);            // DF=18, ES/NT
    setbits(esnt_frame, 6, 8, encode_cf(mdb));// CF
    setbits(esnt_frame, 9, 32, mdb->address); // AA

    supersonic = (mdb->airground_state == AG_SUPERSONIC);
    setbits(esnt_frame+4, 1, 5, 19);               // FORMAT TYPE CODE = 19, airborne velocity
    if (supersonic)
        setbits(esnt_frame+4, 6, 8, 2);            // SUBTYPE = 2, supersonic, speed over ground
    else
        setbits(esnt_frame+4, 6, 8, 1);            // SUBTYPE = 1, subsonic, speed over ground

    setbits(esnt_frame+4, 9, 9, encode_imf(mdb));  // IMF
    setbits(esnt_frame+4, 10, 10, 0);              // IFR
    setbits(esnt_frame+4, 11, 13, 0);              // NAVIGATIONAL UNCERTAINTY CATEGORY FOR VELOCITY

    // EAST/WEST DIRECTION BIT + EAST/WEST VELOCITY
    if (!mdb->ew_vel_valid)
        setbits(esnt_frame+4, 14, 24, 0);                             
    else
        setbits(esnt_frame+4, 14, 24, encode_air_speed(mdb->ew_vel, supersonic));

    // NORTH/SOUTH DIRECTION BIT + NORTH/SOUTH VELOCITY
    if (!mdb->ns_vel_valid)
        setbits(esnt_frame+4, 25, 35, 0);
    else
        setbits(esnt_frame+4, 25, 35, encode_air_speed(mdb->ns_vel, supersonic));

    switch (mdb->vert_rate_source) {
        case ALT_BARO:
            setbits(esnt_frame+4, 36, 36, 0);                                 // SOURCE = BARO
            setbits(esnt_frame+4, 37, 46, encode_vert_rate(mdb->vert_rate));  // SIGN BIT FOR VERTICAL RATE + VERTICAL RATE
            break;

        case ALT_GEO:
            setbits(esnt_frame+4, 36, 36, 1);                                 // SOURCE = GNSS
            setbits(esnt_frame+4, 37, 46, encode_vert_rate(mdb->vert_rate));  // SIGN BIT FOR VERTICAL RATE + VERTICAL RATE
            break;

        default:
            setbits(esnt_frame+4, 36, 36, 0);                                 // SOURCE = BARO
            setbits(esnt_frame+4, 37, 46, 0);                                 // SIGN BIT FOR VERTICAL RATE + VERTICAL RATE = 0, no information
            break;
    }

    setbits(esnt_frame+4, 47, 48, 0);              // RESERVED FOR TURN INDICATOR

    if (mdb->altitude_type != ALT_INVALID && mdb->sec_altitude_type != ALT_INVALID) {
        int delta, sign;

        if (mdb->altitude < mdb->sec_altitude) { // secondary above primary
            delta = mdb->sec_altitude - mdb->altitude;
            sign = mdb->altitude_type == ALT_BARO ? 0 : 1;
        } else { // primary above secondary
            delta = mdb->altitude - mdb->sec_altitude;
            sign = mdb->altitude_type == ALT_BARO ? 1 : 0;
        }

        delta = delta / 25 + 1;
        if (delta >= 127) delta = 127;
        setbits(esnt_frame+4, 49, 49, sign);              // DIFFERENCE SIGN BIT
        setbits(esnt_frame+4, 50, 56, delta);             // GNSS ALT DIFFERENCE FROM BARO ALT
    } else {
        setbits(esnt_frame+4, 49, 49, 0);                 // DIFFERENCE SIGN BIT
        setbits(esnt_frame+4, 50, 56, 0);                 // GNSS ALT DIFFERENCE FROM BARO ALT = 0, no information
    }

    p = checksum_and_print(esnt_frame, 14, 0, p, end);
    return p;
}

// yeah, this could be done with a lookup table, meh.
static char ais_charset[65] = "@ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\]^_ !\"#$%&'()*+,-./0123456789:;<=>?";
static uint8_t char_to_ais(int ch)
{
    char *match;
    if (!ch)
        return 32;

    match = strchr(ais_charset, ch);
    if (match)
        return (uint8_t)(match - ais_charset);
    else
        return 32;
}

static unsigned encodeSquawk(char *squawkStr)
{
    unsigned squawk = strtoul(squawkStr, NULL, 16);
    unsigned encoded = 0;

    if (squawk & 0x1000) encoded |= 0x0800; // A1
    if (squawk & 0x2000) encoded |= 0x0200; // A2
    if (squawk & 0x4000) encoded |= 0x0080; // A4

    if (squawk & 0x0100) encoded |= 0x0020; // B1
    if (squawk & 0x0200) encoded |= 0x0008; // B2
    if (squawk & 0x0400) encoded |= 0x0002; // B4

    if (squawk & 0x0010) encoded |= 0x1000; // C1
    if (squawk & 0x0020) encoded |= 0x0400; // C2
    if (squawk & 0x0040) encoded |= 0x0100; // C4

    if (squawk & 0x0001) encoded |= 0x0010; // D1
    if (squawk & 0x0002) encoded |= 0x0004; // D2
    if (squawk & 0x0004) encoded |= 0x0001; // D4

    return encoded;
}

static int mapSquawkToEmergency(const char *squawkStr)
{
    if (!strcmp(squawkStr, "7500"))
        return 5; // Unlawful Interference
    if (!strcmp(squawkStr, "7600"))
        return 4; // No Communications
    if (!strcmp(squawkStr, "7700"))
        return 1; // General Emergency
    return 0; // No Emergency
}

static char* maybe_send_callsign(struct uat_adsb_mdb *mdb, char *p, char *end)
{
    uint8_t esnt_frame[14] = { 0 };
    int imf = encode_imf(mdb);

    switch (mdb->callsign_type) {
        case CS_CALLSIGN:
            if (imf) {
                // Not sent with non-ICAO addresses
                return p;
            }

            setbits(esnt_frame, 1, 5, 18);            // DF=18, ES/NT
            setbits(esnt_frame, 6, 8, encode_cf(mdb));// CF
            setbits(esnt_frame, 9, 32, mdb->address); // AA

            if (mdb->emitter_category <= 7) {
                setbits(esnt_frame+4, 1, 5, 4);                         // FORMAT TYPE CODE = 4, aircraft category A
                setbits(esnt_frame+4, 6, 8, mdb->emitter_category & 7); // AIRCRAFT CATEGORY (A0 - A7)
            } else if (mdb->emitter_category <= 15) {
                setbits(esnt_frame+4, 1, 5, 3);                         // FORMAT TYPE CODE = 3, aircraft category B
                setbits(esnt_frame+4, 6, 8, mdb->emitter_category & 7); // AIRCRAFT CATEGORY (B0 - B7)
            } else if (mdb->emitter_category <= 23) {
                setbits(esnt_frame+4, 1, 5, 2);                         // FORMAT TYPE CODE = 2, aircraft category C
                setbits(esnt_frame+4, 6, 8, mdb->emitter_category & 7); // AIRCRAFT CATEGORY (C0 - C7)
            } else if (mdb->emitter_category <= 31) {
                setbits(esnt_frame+4, 1, 5, 1);                         // FORMAT TYPE CODE = 1, aircraft category D
                setbits(esnt_frame+4, 6, 8, mdb->emitter_category & 7); // AIRCRAFT CATEGORY (D0 - D7)
            } else {
                // reserved, map to A0
                setbits(esnt_frame+4, 1, 5, 4);                         // FORMAT TYPE CODE = 4, aircraft category A
                setbits(esnt_frame+4, 6, 8, 0);                         // AIRCRAFT CATEGORY A0
            }

            // Map callsign
            setbits(esnt_frame+4, 9, 14, char_to_ais(mdb->callsign[0]));
            setbits(esnt_frame+4, 15, 20, char_to_ais(mdb->callsign[1]));
            setbits(esnt_frame+4, 21, 26, char_to_ais(mdb->callsign[2]));
            setbits(esnt_frame+4, 27, 32, char_to_ais(mdb->callsign[3]));
            setbits(esnt_frame+4, 33, 38, char_to_ais(mdb->callsign[4]));
            setbits(esnt_frame+4, 39, 44, char_to_ais(mdb->callsign[5]));
            setbits(esnt_frame+4, 45, 50, char_to_ais(mdb->callsign[6]));
            setbits(esnt_frame+4, 51, 56, char_to_ais(mdb->callsign[7]));
            p = checksum_and_print(esnt_frame, 14, 0, p, end);
            break;

        case CS_SQUAWK:
            setbits(esnt_frame, 1, 5, 18);            // DF=18, ES/NT
            setbits(esnt_frame, 6, 8, encode_cf(mdb));// CF
            setbits(esnt_frame, 9, 32, mdb->address); // AA

            setbits(esnt_frame+4, 1, 5, 28);                           // FORMAT TYPE CODE = 28, Aircraft Status Message
            setbits(esnt_frame+4, 6, 8, 1);                            // subtype = 1, emergency/priority status
            setbits(esnt_frame+4, 9, 11, mapSquawkToEmergency(mdb->callsign));
            setbits(esnt_frame+4, 12, 24, encodeSquawk(mdb->callsign));
            // 25..55 reserved
            setbits(esnt_frame+4, 56, 56, imf);
            p = checksum_and_print(esnt_frame, 14, 0, p, end);
            break;

        default:
            break;
    }
    return p;
}

// Generator polynomial for the Mode S CRC:
#define MODES_GENERATOR_POLY 0xfff409U

// CRC values for all single-byte messages;
// used to speed up CRC calculation.
static uint32_t crc_table[256];

void uat2esnt_initCrcTables()
{
    int i;
    for (i = 0; i < 256; ++i) {
        uint32_t c = i << 16;
        int j;
        for (j = 0; j < 8; ++j) {
            if (c & 0x800000)
                c = (c<<1) ^ MODES_GENERATOR_POLY;
            else
                c = (c<<1);
        }

        crc_table[i] = c & 0x00ffffff;
    }
}

static uint32_t checksum(uint8_t *message, int n)
{
    uint32_t rem = 0;
    int i;

    for (i = 0; i < n; ++i) {
        rem = (rem << 8) ^ crc_table[message[i] ^ ((rem & 0xff0000) >> 16)];
        rem = rem & 0xffffff;
    }

    return rem;
}

static uint8_t signal_strength = 0;

static char* checksum_and_print(uint8_t *frame, int len, uint32_t parity, char *p, char *end)
{
    // 2 characters for each byte, 16 characters for timestamp / signal / stuff, 8 characters safety buffer :)
    int char_len = 2 * len + 16 + 8;
    int max_len = end - p;
    if (char_len > max_len) {
        fprintf(stderr, "uat2esnt: checksum_and_print(): message too long! (%d / %d)\n", len, max_len);
        return p;
    }

    int j;
    uint32_t rem = checksum(frame, len-3) ^ parity;

    frame[len-3] = (rem & 0xFF0000) >> 16;
    frame[len-2] = (rem & 0x00FF00) >> 8;
    frame[len-1] = (rem & 0x0000FF);

#define MAGIC_UAT_TIMESTAMP  "FF004D4C4155"

    // using format <-AVR: <+beast_ts+sigL+raw+;\n

    // 15 characters
    // print start char, timestamp and signal level <TTTTTTTTTTTTSS
    p += sprintf(p, "<" MAGIC_UAT_TIMESTAMP "%02X", signal_strength);

    // print raw frame
    for (j = 0; j < len; j++) {
        p += sprintf(p, "%02X", frame[j]);
    }

    // 1 character
    p += sprintf(p, ";\n");

    return p;
}

static void generate_esnt(struct uat_adsb_mdb *mdb, float ss, char *p, char *end)
{
    if (!mdb->address)
        return;

    double ss_W = pow(10.0, ss / 10.0);
    int sig = round(sqrt(ss_W) * 255.0);
    if (ss_W > 0 && sig < 1)
        sig = 1;
    if (sig > 255)
        sig = 255;

    signal_strength = (uint8_t)sig;

    p = maybe_send_surface_position(mdb, p, end);
    p = maybe_send_air_position(mdb, p, end);
    p = maybe_send_air_velocity(mdb, p, end);
    p = maybe_send_callsign(mdb, p, end);
}

static int use_tisb = 1;

static int should_send(struct uat_adsb_mdb *mdb)
{
    switch (mdb->address_qualifier) {
        case AQ_ADSB_ICAO:
            return 1; // Real UAT
        case AQ_TISB_ICAO:
        case AQ_TISB_OTHER:
            return use_tisb; // Only if TIS-B is enabled
        default:
            return 1;
    }
}

static void frame_to_esnt(frame_type_t type, uint8_t *frame, int len, char *p, char *end, float ss)
{
    _UNUSED(len);
    *p = '\0';

    if (type == UAT_DOWNLINK) {
        struct uat_adsb_mdb mdb;
        uat_decode_adsb_mdb(frame, &mdb);

        if (should_send(&mdb)) {
            generate_esnt(&mdb, ss, p, end);
        }
    }
}

static int hexbyte(char *buf)
{
    int i;
    char c;

    c = buf[0];
    if (c >= '0' && c <= '9')
        i = (c - '0');
    else if (c >= 'a' && c <= 'f')
        i = (c - 'a' + 10);
    else if (c >= 'A' && c <= 'F')
        i = (c - 'A' + 10);
    else
        return -1;

    i <<= 4;
    c = buf[1];
    if (c >= '0' && c <= '9')
        return i | (c - '0');
    else if (c >= 'a' && c <= 'f')
        return i | (c - 'a' + 10);
    else if (c >= 'A' && c <= 'F')
        return i | (c - 'A' + 10);
    else
        return -1;
}

static int process_line(frame_handler_t handler, void *handler_data1, void *handler_data2, char *p, char *end)
{
    uint8_t frame_back[UPLINK_FRAME_DATA_BYTES];
    uint8_t *frame = frame_back;

    int len = 0;
    frame_type_t frametype;

    if (*p == '-')
        frametype = UAT_DOWNLINK;
    else if (*p == '+')
        frametype = UAT_UPLINK;
    else
        return 0;

    ++p;
    while (p < end) {
        int byte;

        /*
         * If we hit a semicolon, the rest of the line is "extra info", like timestmap, RSSI, etc.
         * Try to parse some of it...
         */
        if (p[0] == ';') {
            int done = 0;
            float signal_strength = 0;
            p++;

            while (*p && !done) {
                if (!strncmp(p, "ss=", 3))
                    sscanf(p, "ss=%f;", &signal_strength);
                if (!strncmp(p, "rssi=", 5))
                    sscanf(p, "rssi=%f;", &signal_strength);
                // if (!strncmp(p, "t=", 2))
                //     sscanf(p, "t=%f;", &timestamp);
                /* Advance until next semicolon (or end)... */
                while (*p && *p != ';') { p++; }

                // Short-circuit and bail if we have SS - remove later if we want to parse other info
                if (!*p || (signal_strength != 0)) {
                    done = 1;
                } else {
                    p++;
                }
            }

            // ignore rest of line
            handler(frametype, frame_back, len, handler_data1, handler_data2, signal_strength);
            return 1;
        }

        if (len >= (int) sizeof(frame_back)) {
            return 0; // oversized frame
        }
        byte = hexbyte(p);
        if (byte < 0) {
            return 0; // badly formatted byte
        }
        ++len;
        *frame++ = byte;
        p += 2;
    }

    return 0; // ran off the end without seeing semicolon
}


void uat2esnt_convert_message(char *p, char *end, char *out, char *out_end) {
    out[0] = '\0';
    process_line(frame_to_esnt, out, out_end, p, end);
}
