#include <string.h>
#include <errno.h>

#include <patty/ax25.h>

static ssize_t decode_station(patty_ax25_addr *addr,
                              void *data,
                              off_t offset) {
    int i, space = 0;

    for (i=0; i<PATTY_AX25_CALLSIGN_LEN; i++) {
        uint8_t b = ((uint8_t *)data + offset)[i];
        uint8_t c = b >> 1;

        if (!PATTY_AX25_ADDRESS_CHAR_VALID(c) || PATTY_AX25_ADDRESS_OCTET_LAST(b)) {
            errno = EINVAL;

            goto error;
        }

        if (c == ' ' && !space) {
            space = 1;
        } else if (c != ' ' && space) {
            errno = EINVAL;

            goto error;
        }
    }

    memcpy(addr, ((uint8_t *)data) + offset, sizeof(*addr));

    return PATTY_AX25_ADDRESS_SIZE;

error:
    return -1;
}

static ssize_t decode_hops(patty_ax25_frame *frame,
                           void *data,
                           off_t start) {
    ssize_t decoded;
      off_t offset = start;

    int i;

    patty_ax25_addr *addr = NULL;

    /*
     * Try to count the AX.25-specified maximum number of hops in the current
     * frame.
     */
    for (i=0; i<PATTY_AX25_MAX_HOPS; i++) {
        addr = (patty_ax25_addr *)((uint8_t *)data + offset);

        if ((decoded = decode_station(&frame->repeaters[i], data, offset)) < 0) {
            goto error;
        } else {
            offset += decoded;
        }

        frame->hops++;

        if (PATTY_AX25_ADDRESS_OCTET_LAST(addr->ssid)) {
            break;
        }
    }

    /*
     * If the last hop does not have the address extension bit set, then
     * that's a big problem.
     */
    if (addr && !PATTY_AX25_ADDRESS_OCTET_LAST(addr->ssid)) {
        errno = EINVAL;

        goto error;
    }

    return offset - start;

error:
    return -1;
}

static ssize_t decode_address(patty_ax25_frame *frame,
                              void *data,
                              off_t start) {
      off_t offset = start;
    ssize_t decoded;

    if ((decoded = decode_station(&frame->dest, data, offset)) < 0) {
        goto error;
    } else {
        offset += decoded;
    }

    if ((decoded = decode_station(&frame->src, data, offset)) < 0) {
        goto error;
    } else {
        offset += decoded;
    }

    /*
     * If the source address is not the final address in the frame, begin
     * decoding repeater addresses.
     */
    if (!PATTY_AX25_ADDRESS_OCTET_LAST(frame->src.ssid)) {
        if ((decoded = decode_hops(frame, data, offset)) < 0) {
            goto error;
        } else {
            offset += decoded;
        }
    }

    return offset - start;

error:
    return -1;
}

static ssize_t decode_info(patty_ax25_frame *frame,
                           void *data,
                           off_t offset,
                           size_t size) {
    uint8_t control = ((uint8_t *)data + offset)[0];
     size_t decoded = 0;

    int info = 0;

    if (PATTY_AX25_CONTROL_INFO(control)) {
        info = 1;
    } else if (PATTY_AX25_CONTROL_UNNUMBERED(control)) {
        if (PATTY_AX25_CONTROL_UNNUMBERED_INFO(control)) {
            info = 1;
        }
    } else if (!PATTY_AX25_CONTROL_SUPER(control)) {
        errno = EINVAL;

        goto error;
    }

    frame->control = control;

    decoded++;

    /*
     * If we've received either an Info or Unnumbered Info frame, then decode
     * the Protocol Identifier field, and calculate the location and size of
     * the payload as well.
     */
    if (info) {
        decoded++;

        frame->proto   = ((uint8_t *)data + offset)[1];
        frame->info    = (void *)((uint8_t *)data + offset + decoded);
        frame->infolen = size - offset - decoded;

        decoded += frame->infolen;
    }

    return decoded;

error:
    return -1;
}

ssize_t patty_ax25_frame_decode(patty_ax25_frame *frame,
                                enum patty_ax25_frame_format format,
                                void *buf,
                                size_t size) {
    ssize_t decoded;
    off_t offset = 0;

    memset(frame, '\0', sizeof(*frame));

    /*
     * First, decode the variable-length Address field.
     */
    if ((decoded = decode_address(frame, buf, offset)) < 0) {
        errno = EIO;

        goto error_decode;
    } else {
        offset += decoded;
    }

    /*
     * Next, decode the remaining Control Field, optional Protocol Identifier
     * field, and Info payload that may follow.
     */
    if ((decoded = decode_info(frame, buf, offset, size)) < 0) {
        errno = EIO;

        goto error_decode;
    } else {
        offset += decoded;
    }

    return offset;

error_decode:
    return -1;
}

static ssize_t encode_address(void *buf,
                              patty_ax25_addr *dest,
                              patty_ax25_addr *src,
                              patty_ax25_addr *repeaters,
                              unsigned int hops,
                              size_t len) {
    size_t offset = 0;
    unsigned int i;

    if (hops > PATTY_AX25_MAX_HOPS) {
        hops = PATTY_AX25_MAX_HOPS;
    }

    if (len < (2 + hops) * sizeof(patty_ax25_addr)) {
        goto error_toobig;
    }

    memcpy((uint8_t *)dest + offset, &dest, sizeof(patty_ax25_addr));
    offset += sizeof(patty_ax25_addr);
    ((uint8_t *)dest)[offset-1] &= ~1;

    memcpy((uint8_t *)dest + offset, &src, sizeof(patty_ax25_addr));
    offset += sizeof(patty_ax25_addr);
    ((uint8_t *)dest)[offset-1] &= ~1;

    for (i=0; i<hops; i++) {
        if (repeaters[i].callsign[0] == '\0') {
            break;
        }

        memcpy((uint8_t *)dest + offset,
               &repeaters[i],
               sizeof(patty_ax25_addr));

        offset += sizeof(patty_ax25_addr);
        ((uint8_t *)dest)[offset-1] &= ~1;
    }

    ((uint8_t *)dest)[offset-1] |= 1;

    return offset;

error_toobig:
    return -1;
}

static ssize_t encode_reply_address(patty_ax25_frame *frame,
                                    void *dest,
                                    size_t len) {
    size_t i,
           offset = 0,
           hops   = frame->hops > PATTY_AX25_MAX_HOPS?
                                  PATTY_AX25_MAX_HOPS: frame->hops;

    if ((2 + hops) * sizeof(patty_ax25_addr) > len) {
        goto error_toobig;
    }

    memcpy((uint8_t *)dest + offset, &frame->src, sizeof(patty_ax25_addr));
    offset += sizeof(patty_ax25_addr);
    ((uint8_t *)dest)[offset-1] &= ~1;

    memcpy((uint8_t *)dest + offset, &frame->dest, sizeof(patty_ax25_addr));
    offset += sizeof(patty_ax25_addr);
    ((uint8_t *)dest)[offset-1] &= ~1;

    for (i=0; i<hops; i++) {
        if (frame->repeaters[i].callsign[0] == '\0') {
            break;
        }

        memcpy((uint8_t *)dest + offset,
               &frame->repeaters[hops-1-i],
               sizeof(patty_ax25_addr));

        offset += sizeof(patty_ax25_addr);
        ((uint8_t *)dest)[offset-1] &= ~1;
    }

    ((uint8_t *)dest)[offset-1] |= 1;

    return offset;

error_toobig:
    return -1;
}

ssize_t patty_ax25_frame_encode(patty_ax25_frame *frame,
                                void *buf,
                                enum patty_ax25_frame_format format,
                                uint16_t control,
                                uint8_t proto,
                                void *info,
                                size_t infolen,
                                size_t len) {
    size_t offset;

    offset = encode_address(buf,
                            &frame->dest,
                            &frame->src,
                             frame->repeaters,
                             frame->hops,
                             len);

    switch (format) {
        case PATTY_AX25_FRAME_NORMAL:
            ((uint8_t *)buf)[offset++] = frame->control;

            break;

        case PATTY_AX25_FRAME_EXTENDED:
            ((uint8_t *)buf)[offset++] = (frame->control & 0xff00) >> 8;
            ((uint8_t *)buf)[offset++] =  frame->control & 0xf00ff;

            break;
    }

    if (PATTY_AX25_CONTROL_INFO(frame->control)) {
        if (1 + offset + frame->infolen > len) {
            goto error_toobig;
        }

        ((uint8_t *)buf)[offset++] = frame->proto;

        memcpy((uint8_t *)buf + offset, frame->info, frame->infolen);

        offset += frame->infolen;
    }

    return offset;

error_toobig:
    return -1;
}

ssize_t patty_ax25_frame_encode_reply_to(patty_ax25_frame *frame,
                                         void *buf,
                                         enum patty_ax25_frame_format format,
                                         uint16_t control,
                                         uint8_t proto,
                                         void *info,
                                         size_t infolen,
                                         size_t len) {
    size_t offset = 0;

    offset = encode_reply_address(frame, buf, len);

    switch (format) {
        case PATTY_AX25_FRAME_NORMAL:
            ((uint8_t *)buf)[offset++] = control;

            break;

        case PATTY_AX25_FRAME_EXTENDED:
            ((uint8_t *)buf)[offset++] = (control & 0xff00) >> 8;
            ((uint8_t *)buf)[offset++] =  control & 0xf00ff;

            break;
    }

    if (PATTY_AX25_CONTROL_INFO(control)) {
        if (1 + offset + infolen > len) {
            goto error_toobig;
        }

        ((uint8_t *)buf)[offset++] = proto;

        memcpy((uint8_t *)buf + offset, info, infolen);

        offset += infolen;
    }

    return offset;

error_toobig:
    return -1;
}

enum patty_ax25_version patty_ax25_frame_version(patty_ax25_frame *frame) {
    return PATTY_AX25_ADDRESS_SSID_C(frame->src.ssid) ==
           PATTY_AX25_ADDRESS_SSID_C(frame->dest.ssid)?
               PATTY_AX25_2_0: PATTY_AX25_OLD;
}

enum patty_ax25_frame_type patty_ax25_frame_type(patty_ax25_frame *frame) {
    if (PATTY_AX25_CONTROL_INFO(frame->control)) {
        return PATTY_AX25_FRAME_INFO;
    } else if (PATTY_AX25_CONTROL_SUPER(frame->control)) {
        return PATTY_AX25_FRAME_SUPER;
    } else if (PATTY_AX25_CONTROL_UNNUMBERED(frame->control)) {
        return PATTY_AX25_FRAME_UNNUMBERED;
    }

    return PATTY_AX25_FRAME_UNKNOWN;
}

enum patty_ax25_frame_cr patty_ax25_frame_cr(patty_ax25_frame *frame) {
    return PATTY_AX25_ADDRESS_SSID_C(frame->src.ssid)?
        PATTY_AX25_FRAME_RESPONSE:
        PATTY_AX25_FRAME_COMMAND;
}

ssize_t patty_ax25_frame_info(patty_ax25_frame *frame,
                              void **info) {
    if (frame == NULL || frame->info == NULL || info == NULL) {
        errno = EINVAL;

        goto error_invalid_args;
    }

    *info = frame->info;

    return frame->infolen;

error_invalid_args:
    return -1;
}