#include "coap.h"

namespace coap {

uint8_t CoapMessage::getVersion() {
    return vertkl >> 6;
}

uint8_t CoapMessage::getType() {
    return (vertkl & 0x30) >> 4;
}

uint8_t CoapMessage::getTKL() {
    return (vertkl & 0x0f);
}

uint8_t CoapMessage::getCode() {
    return code;
}

const char* CoapMessage::getMID() {
    return id;
}

/* nonsense: save 8 bits in the network,
   and waste tens of them for writing such an error-prone code like this! */
int CoapMessage::getOptionDelta(const unsigned char* option) {
    uint8_t delta = option[0] >> 4;
    if (delta <= 12) {
        return delta;
    }

    if (delta == 13) {
        delta = option[1] + 13;
        return delta;
    }

    /* options delta larger than 269 (256 + 13) bytes are not supported */
    return -1;
}
int CoapMessage::getOptionLength(const unsigned char* option) {
    uint8_t len = option[0] & 0xf;

    if (len <= 12) {
        return len;
    }

    uint8_t extended_length_byte;
    if (getOptionDelta(option) < 13)
        extended_length_byte = option[1];
    else
        extended_length_byte = option[2];

    if (len == 13) {
        len = extended_length_byte + 13;
        return len;
    }

    /* options longer than 269 (256 + 13) are not supported */
    return -1;
}
/* offset of payload wrt beginning of option */
int CoapMessage::getOptionValueOffset(const unsigned char* option) {
    int offset = 1;
    if (getOptionDelta(option) >= 13) ++offset;
    if (getOptionLength(option) >= 13) ++offset;
    return offset;
}
const char* CoapMessage::getOptionValue(const unsigned char* option) {
    return (const char*) &option[getOptionValueOffset(option)];
}
const unsigned char* CoapMessage::getOptions() {
    const unsigned char* address = (const unsigned char*)this;
    address += 4;  /* header */
    address += getTKL();
    return address;
}

void CoapMessage::getUriPath(char* output_buffer, const char* last) {
    /* TODO -- possible overflow, malformed input not checked */
    int number = 0;
    int len;
    const unsigned char* options = this->getOptions();

    while (number <= 11) {
        number += getOptionDelta(options);
        len = getOptionLength(options);
        // TODO -- is here a payload? 15 - 15
        if (number == 11) { /* Uri-Path option number */
            *output_buffer = '/';
            output_buffer += 1;
            memcpy(output_buffer, getOptionValue(options), len);
            output_buffer += len;
        }
        options += (len + getOptionValueOffset(options));
        if (options == last) break;
    }

    *output_buffer = '\0';

    return;
}

int coap_server(char* output_buffer, const char* input_buffer, int len) {
    CoapMessage* input_msg = (CoapMessage*) input_buffer;
    if (input_msg->getVersion() != 0x1) /* bad CoAP version number */
        return -1;

    if (input_msg->getType() == RESET || input_msg->getType() == ACKNOWELEDGMENT) /* not implemented on server side */
        return -1;

    if (input_msg->getTKL() >= 9)   /* format error -- TODO, maybe we can answer RST? see RFC */
        return -1;

    switch (input_msg->getCode()) {
        case GET:
            input_msg->getUriPath(output_buffer, input_buffer + len);
            return strlen(output_buffer);
            break;
    }
    return -1;
}

} // end of namespace -- coap::