#include <stdio.h>

#include "gsm.h"

#define DIVC(x,y)                   ((x) / (y) + ((x) % (y) != 0))

size_t pdu_putc(char *pdu, uint8_t ch) {
    char b[3];

    sprintf(b, "%.2X", ch);
    *pdu = b[0];
    *(pdu + 1) = b[1];

    return 2;
}

ssize_t pdu_getc(char *pdu, uint8_t *ch) {
    int rv, _ch;

    rv = sscanf(pdu, "%2X", &_ch);
    if (rv != 1) return GSM_ERR;

    *ch = _ch;
    return 2;
}

size_t pdu_puts(char *pdu, uint8_t *s, size_t s_len) {
    int i;
    char b[3];

    for (i=0; i<s_len; i++) {
        sprintf(b, "%.2X", s[i]);
        *(pdu + 2 * i) = b[0];
        *(pdu + 2 * i + 1) = b[1];
    }

    return s_len * 2;
}

ssize_t pdu_gets(char *pdu, uint8_t *s, size_t s_len) {
    int i, rv, _ch;

    for (i=0; i<s_len; i++) {
        rv = sscanf(pdu + 2 * i, "%2X", &_ch);
        if (rv != 1) return GSM_ERR;
        s[i] = _ch;
    }

    return s_len * 2;
}

size_t pdu_7bit_enc(char *pdu, char *text, size_t text_len, int padb) {
    uint8_t carry = 0;
    size_t pdu_len = 0;
    int i = 0, shc = 0;

    if (!text_len) return 0;

    if (padb) {
        shc = 7 - padb;
    } else {
        carry = *text;
        i++;
    }

    while (i < text_len) {
        pdu_putc(pdu + pdu_len, carry | (*(text + i) << (7 - shc)));
        pdu_len += 2;

        shc++;
        shc = shc % 7;
        if (!shc) {
            i++;
            if (i == text_len) return pdu_len;
        }

        carry = *(text + i) >> shc;
        i++;
    }
    pdu_putc(pdu + pdu_len, carry);
    pdu_len += 2;

    return pdu_len;
}

ssize_t pdu_7bit_dec(char *pdu, char *text, size_t text_len, int padb) {
    uint8_t ch;
    uint8_t carry = 0;
    int i = 0, shc = 0;
    size_t pdu_len = 0;
    ssize_t rv;

    if (!text_len) return 0;

    if (padb) {
        rv = pdu_getc(pdu, &ch);
        if (rv < 0) return rv;
        pdu_len += 2;

        if (padb == 1) {
            *text = ch >> 1;
            i++;
        } else {
            carry = ch >> padb;
            shc = 8 - padb;
        }
    }

    while (i < text_len) {
        rv = pdu_getc(pdu + pdu_len, &ch);
        if (rv < 0) return rv;
        pdu_len += 2;

        *(text + i) = ((ch << shc) | carry) & 0x7f;
        carry = ch >> (7 - shc);
        i++;

        shc++;
        shc = shc % 7;
        if (!shc && (i < text_len)) {
            *(text + i) = carry;
            carry = 0;
            i++;
        }
    }

    return pdu_len;
}

void gsm_dcs_enc(uint8_t enc, uint16_t flags, uint8_t *dcs) {
    *dcs = enc;
    if (flags & GSM_FLAG_CLASS) {
        *dcs |= GSM_DCS_CLASS_IND;
        *dcs |= (flags >> 8) & GSM_DCS_CLASS;
    }
    if (flags & GSM_FLAG_COMPRESS) *dcs |= GSM_DCS_COMPRESS_IND;
    if (flags & GSM_FLAG_DELETE) *dcs |= GSM_DCS_DELETE_IND;
}

void gsm_dcs_dec(uint8_t dcs, uint8_t *enc, uint16_t *flags) {
    if ((dcs & GSM_DCS_GENERAL_IND) == 0) {
        *enc = dcs & GSM_DCS_ENC;
        if (dcs & GSM_DCS_CLASS_IND) {
            *flags |= GSM_FLAG_CLASS;
            *flags |= (uint16_t)(dcs & GSM_DCS_CLASS) << 8;
        }
        if (dcs & GSM_DCS_COMPRESS_IND) *flags |= GSM_FLAG_COMPRESS;
        if (dcs & GSM_DCS_DELETE_IND) *flags |= GSM_FLAG_DELETE;
    } else {
        uint8_t group = dcs & GSM_DCS_GROUP;

        switch (group) {
            case GSM_DCS_MWI_DISCARD:
            case GSM_DCS_MWI_STORE_GSM7:
            case GSM_DCS_MWI_STORE_UCS2: {
                if (group == GSM_DCS_MWI_STORE_UCS2) {
                    *enc = GSM_ENC_UCS2;
                } else {
                    *enc = GSM_ENC_7BIT;
                }
                if (GSM_DCS_MWI_DISCARD) *flags |= GSM_FLAG_DISCARD;
                *flags |= GSM_FLAG_MWI;
                *flags |= (uint16_t)(dcs & (GSM_DCS_MWI_SENSE | GSM_DCS_MWI_TYPE)) << 12;
                break;
            }

            case GSM_DCS_ENCLASS: {
                *flags |= GSM_FLAG_CLASS;
                *flags |= (uint16_t)(dcs & GSM_DCS_CLASS) << 8;
                *enc = dcs & GSM_DCS_ENCLASS_ENC ? GSM_ENC_8BIT : GSM_ENC_7BIT;
                break;
            }
        }
    }
}

ssize_t gsm_ts_enc(char *pdu, size_t pdu_size, char *ts) {
    uint8_t tz;
    int tz_hh, tz_mm;

    if (pdu_size < 14) return GSM_ERR_SIZE;

    pdu[1]  = ts[2];    // YY
    pdu[0]  = ts[3];

    pdu[3]  = ts[5];    // MM
    pdu[2]  = ts[6];

    pdu[5]  = ts[8];    // DD
    pdu[4]  = ts[9];

    pdu[7]  = ts[11];   // hh
    pdu[6]  = ts[12];

    pdu[9]  = ts[14];   // mm
    pdu[8]  = ts[15];

    pdu[11] = ts[17];   // ss
    pdu[10] = ts[18];

    sscanf(ts + 20, "%2d:%2d", &tz_hh, &tz_mm);
    tz = tz_hh * 4 + tz_mm / 15;
    tz = (tz / 10) | ((tz % 10) << 4);
    if (ts[19] == '-') tz |= 0x08;

    pdu_putc(pdu + 12, tz);

    return 14;
}

ssize_t gsm_ts_dec(char *pdu, size_t pdu_len, char *ts) {
    uint8_t tz;
    ssize_t rv;

    if (pdu_len < 14) return GSM_ERR_SIZE;

    ts[0]  = '2';
    ts[1]  = '0';
    ts[2]  = pdu[1];    // YY
    ts[3]  = pdu[0];
    ts[4]  = '-';
    ts[5]  = pdu[3];    // MM
    ts[6]  = pdu[2];
    ts[7]  = '-';
    ts[8]  = pdu[5];    // DD
    ts[9]  = pdu[4];
    ts[10] = 'T';
    ts[11] = pdu[7];    // hh
    ts[12] = pdu[6];
    ts[13] = ':';
    ts[14] = pdu[9];    // mm
    ts[15] = pdu[8];
    ts[16] = ':';
    ts[17] = pdu[11];   // ss
    ts[18] = pdu[10];

    rv = pdu_getc(pdu + 12, &tz);
    if (rv < 0) return rv;

    if (tz & 0x08) {
        ts[19] = '-';
        tz = tz & ~0x08;
    } else {
        ts[19] = '+';
    }
    tz = (tz & 0x0f) * 10 + (tz >> 4);
    sprintf(ts + 20, "%.2d:%.2d", tz / 4, (tz % 4) * 15);

    return 14;
}

ssize_t gsm_addr_enc(char *pdu, size_t pdu_size, char *addr, size_t addr_len, uint8_t addr_type) {
    size_t _pdu_len;

    addr_type |= GSM_EXT;

    if ((addr_type & GSM_TON) == GSM_TON_ALPHANUMERIC) {
        int _addr_len = DIVC(addr_len * 7, 4);

        _pdu_len = 4 + DIVC(_addr_len, 2) * 2;
        if (pdu_size < _pdu_len) return GSM_ERR_SIZE;

        pdu_putc(pdu, _addr_len);
        pdu_putc(pdu + 2, addr_type);
        pdu_7bit_enc(pdu + 4, addr, addr_len, 0);
    } else {
        int i;

        _pdu_len = 4 + DIVC(addr_len, 2) * 2;
        if (pdu_size < _pdu_len) return GSM_ERR_SIZE;

        pdu_putc(pdu, addr_len);
        pdu_putc(pdu + 2, addr_type);
        for (i=0; i<addr_len / 2; i++) {
            pdu[4 + 2 * i] = addr[2 * i + 1];
            pdu[4 + 2 * i + 1] = addr[2 * i];
        }
        if (addr_len % 2 != 0) {
            pdu[4 + 2 * i] = 'F';
            pdu[4 + 2 * i + 1] = addr[2 * i];
        }
    }

    return _pdu_len;
}

ssize_t gsm_addr_dec(char *pdu, size_t pdu_len, char *addr, size_t *addr_len, uint8_t *addr_type) {
    uint8_t pdu_ch;
    size_t _pdu_len;
    ssize_t rv;

    if (pdu_len < 4) return GSM_ERR_SIZE;

    rv = pdu_getc(pdu, &pdu_ch);
    if (rv < 0) return rv;
    rv = pdu_getc(pdu + 2, addr_type);
    if (rv < 0) return rv;

    *addr_len = pdu_ch;
    if (*addr_len > GSM_ADDR_SIZE) return GSM_ERR_SIZE;

    if (!(*addr_type & GSM_EXT)) return GSM_ERR_NOTSUPPORTED;

    _pdu_len = 4 + DIVC(*addr_len, 2) * 2;
    if (pdu_len < _pdu_len) return GSM_ERR_SIZE;

    if ((*addr_type & GSM_TON) == GSM_TON_ALPHANUMERIC) {
        *addr_len = (*addr_len * 4) / 7;

        rv = pdu_7bit_dec(pdu + 4, addr, *addr_len, 0);
        if (rv < 0) return rv;
    } else {
        int i;

        for (i=0; i<*addr_len / 2; i++) {
            addr[2 * i] = pdu[4 + 2 * i + 1];
            addr[2 * i + 1] = pdu[4 + 2 * i];
        }
        if (*addr_len % 2 != 0) {
            addr[2 * i] = pdu[4 + 2 * i + 1];
        }
    }

    return _pdu_len;
}

ssize_t gsm_sms_enc(char *pdu, size_t pdu_size, char *addr, size_t addr_len, uint8_t addr_type, uint8_t *udh, size_t udh_len, uint8_t *txt, size_t txt_len, uint8_t enc, uint16_t flags) {
    size_t _pdu_len = 0;
    uint8_t mti;
    uint8_t mr;
    uint8_t pid;
    uint8_t dcs;
    uint8_t udl;
    ssize_t rv;

    mti = GSM_MTI_SUBMIT;
    if (udh_len) mti |= GSM_UDHI;
    if (flags & GSM_FLAG_STATUS_REPORT) mti |= GSM_SRR;
    if (flags & GSM_FLAG_REPLY_PATH) mti |= GSM_RP;
    mr = 0;

    if (pdu_size < 4) return GSM_ERR_SIZE;
    pdu_putc(pdu, mti);
    pdu_putc(pdu + 2, mr);
    _pdu_len += 4;

    rv = gsm_addr_enc(pdu + _pdu_len, pdu_size - _pdu_len, addr, addr_len, addr_type);
    if (rv < 0) return rv;
    _pdu_len += rv;

    if (pdu_size < _pdu_len + 4) return GSM_ERR_SIZE;
    if (flags & GSM_FLAG_TYPE0) {
        pid = GSM_PID_TYPE0;
    } else {
        pid = GSM_PID_DEFAULT;
    }
    gsm_dcs_enc(enc, flags, &dcs);

    pdu_putc(pdu + _pdu_len, pid);
    pdu_putc(pdu + _pdu_len + 2, dcs);
    _pdu_len += 4;

    if (enc == GSM_ENC_7BIT) {
        int udh_blen = 0;
        int padb = 0;

        if (udh_len) {
            udh_blen = 8 * (udh_len + 1);
            padb = DIVC(udh_blen, 7) * 7 - udh_blen;
        }
        udl = DIVC(udh_blen, 7) + txt_len;

        if (pdu_size < _pdu_len + (DIVC(udl * 7, 8) + 1) * 2) return GSM_ERR_SIZE;
        pdu_putc(pdu + _pdu_len, udl);
        _pdu_len += 2;

        if (udh_len) {
            pdu_putc(pdu + _pdu_len, udh_len);
            pdu_puts(pdu + _pdu_len + 2, udh, udh_len);
            _pdu_len += (udh_len + 1) * 2;
        }

        rv = pdu_7bit_enc(pdu + _pdu_len, (char *)txt, txt_len, padb);
        if (rv < 0) return rv;
        _pdu_len += rv;
    } else {
        udl = txt_len + (udh_len ? udh_len + 1 : 0);

        if (pdu_size < _pdu_len + (udl + 1) * 2) return GSM_ERR_SIZE;
        pdu_putc(pdu + _pdu_len, udl);
        _pdu_len += 2;

        if (udh_len) {
            pdu_putc(pdu + _pdu_len, udh_len);
            pdu_puts(pdu + _pdu_len + 2, udh, udh_len);
            _pdu_len += (udh_len + 1) * 2;
        }

        pdu_puts(pdu + _pdu_len, txt, txt_len);
        _pdu_len += txt_len * 2;
    }

    return _pdu_len;
}

ssize_t gsm_sms_dec(char *pdu, size_t pdu_len, char *addr, size_t *addr_len, uint8_t *addr_type, uint8_t *udh, size_t *udh_len, uint8_t *txt, size_t *txt_len, uint8_t *enc, char *ts, uint16_t *flags) {
    size_t _pdu_len = 0;
    uint8_t mti;
    uint8_t pid;
    uint8_t dcs;
    uint8_t udl;
    ssize_t rv;

    *enc = 0xff;
    *flags = 0;
    if (pdu_len < 2) return GSM_ERR_SIZE;
    rv = pdu_getc(pdu, &mti);
    if (rv < 0) return rv;
    _pdu_len += 2;

    if ((mti & GSM_MTI) != GSM_MTI_DELIVER) return GSM_ERR_NOTSUPPORTED;
    if (mti & GSM_SRI) *flags |= GSM_FLAG_STATUS_REPORT;
    if (mti & GSM_RP) *flags |= GSM_FLAG_REPLY_PATH;

    rv = gsm_addr_dec(pdu + _pdu_len, pdu_len - _pdu_len, addr, addr_len, addr_type);
    if (rv < 0) return rv;
    _pdu_len += rv;

    if (pdu_len < _pdu_len + 4) return GSM_ERR_SIZE;
    rv = pdu_getc(pdu + _pdu_len, &pid);
    if (rv < 0) return rv;
    if (pid == GSM_PID_TYPE0) {
        *flags |= GSM_FLAG_TYPE0;
    } else if (pid != GSM_PID_DEFAULT) {
        return GSM_ERR_NOTSUPPORTED;
    }
    rv = pdu_getc(pdu + _pdu_len + 2, &dcs);
    if (rv < 0) return rv;
    gsm_dcs_dec(dcs, enc, flags);
    _pdu_len += 4;

    rv = gsm_ts_dec(pdu + _pdu_len, pdu_len - _pdu_len, ts);
    if (rv < 0) return rv;
    _pdu_len += rv;

    if (pdu_len < _pdu_len + 2) return GSM_ERR_SIZE;
    rv = pdu_getc(pdu + _pdu_len, &udl);
    if (rv < 0) return rv;
    _pdu_len += 2;

    if ((mti & GSM_UDHI) && (udl == 0)) return GSM_ERR;
    *udh_len = 0;

    if (*enc == GSM_ENC_7BIT) {
        int udh_blen = 0;
        int padb = 0;

        if (pdu_len < _pdu_len + DIVC(udl * 7, 8) * 2) return GSM_ERR_SIZE;

        if (mti & GSM_UDHI) {
            uint8_t pdu_ch;

            rv = pdu_getc(pdu + _pdu_len, &pdu_ch);
            if (rv < 0) return rv;

            *udh_len = pdu_ch;
            udh_blen = 8 * (*udh_len + 1);
            padb = DIVC(udh_blen, 7) * 7 - udh_blen;

            if (udl * 7 < udh_blen) return GSM_ERR;
            if (*udh_len > GSM_UDH_SIZE) return GSM_ERR_SIZE;

            rv = pdu_gets(pdu + _pdu_len + 2, udh, *udh_len);
            if (rv < 0) return rv;
            _pdu_len += (*udh_len + 1) * 2;
        } else {
            *udh_len = 0;
        }

        *txt_len = udl - DIVC(udh_blen, 7);
        if (*txt_len > GSM_UD_SIZE) return GSM_ERR_SIZE;

        rv = pdu_7bit_dec(pdu + _pdu_len, (char *)txt, *txt_len, padb);
        if (rv < 0) return rv;
        _pdu_len += rv;
    } else {
        if (pdu_len < _pdu_len + udl * 2) return GSM_ERR_SIZE;

        if (mti & GSM_UDHI) {
            uint8_t pdu_ch;

            rv = pdu_getc(pdu + _pdu_len, &pdu_ch);
            if (rv < 0) return rv;

            *udh_len = pdu_ch;
            if (udl < *udh_len + 1) return GSM_ERR;
            if (*udh_len > GSM_UDH_SIZE) return GSM_ERR_SIZE;

            rv = pdu_gets(pdu + _pdu_len + 2, udh, *udh_len);
            if (rv < 0) return rv;
            _pdu_len += (*udh_len + 1) * 2;
        } else {
            *udh_len = 0;
        }

        *txt_len = udl - (*udh_len ? *udh_len + 1 : 0);
        if (*txt_len > GSM_UD_SIZE) return GSM_ERR_SIZE;
        if ((*enc == GSM_ENC_UCS2) && ((*txt_len % 2) != 0)) return GSM_ERR;

        rv = pdu_gets(pdu + _pdu_len, txt, *txt_len);
        if (rv < 0) return rv;
        _pdu_len += *txt_len * 2;
    }

    return _pdu_len;
}