/*
 * Copyright (c) 2016 Intel Corporation
 * Copyright (c) 2015 Jules Dourlens (jdourlens@gmail.com)
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "firmata/firmata.h"
#include "mraa_internal.h"

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

t_firmata*
firmata_new(const char* name)
{
    t_firmata* res;

    res = calloc(1, sizeof(t_firmata));
    if (!res) {
        return NULL;
    }

    int ret = pthread_spin_init(&res->lock, PTHREAD_PROCESS_SHARED);
    if (ret != 0) {
        syslog(LOG_ERR, "firmata; could not init locking");
        free(res);
        return NULL;
    }

    res->uart = mraa_uart_init_raw(name);
    if (res->uart == NULL) {
        syslog(LOG_ERR, "firmata: UART failed to setup");
        free(res);
        return  NULL;
    }

    firmata_initPins(res);

    if (mraa_uart_set_baudrate(res->uart, 57600) != MRAA_SUCCESS) {
        syslog(LOG_WARNING, "firmata: Failed to set correct baud rate on %s", name);
    }

    firmata_askFirmware(res);
    syslog(LOG_INFO, "firmata: Device opened at: %s", name);

    return res;
}

void
firmata_close(t_firmata* firmata)
{
    mraa_uart_stop(firmata->uart);
    free(firmata);
}

int
firmata_pull(t_firmata* firmata)
{
    char buff[FIRMATA_MSG_LEN];
    int r;

    r = mraa_uart_data_available(firmata->uart, 40);
    if (r > 0) {
        r = mraa_uart_read(firmata->uart, buff, sizeof(buff));
        if (r < 0) {
            return 0;
        }
        if (r > 0) {
            firmata_parse(firmata, (uint8_t*) buff, r);
            return r;
        }
    }
    return r;
}

void
firmata_parse(t_firmata* firmata, const uint8_t* buf, int len)
{
    const uint8_t* p;
    const uint8_t* end;

    p = buf;
    end = p + len;
    for (p = buf; p < end; p++) {
        uint8_t msn = *p & 0xF0;
        if (msn == 0xE0 || msn == 0x90 || *p == 0xF9) {
            firmata->parse_command_len = 3;
            firmata->parse_count = 0;
        } else if (msn == 0xC0 || msn == 0xD0) {
            firmata->parse_command_len = 2;
            firmata->parse_count = 0;
        } else if (*p == FIRMATA_START_SYSEX) {
            firmata->parse_count = 0;
            firmata->parse_command_len = sizeof(firmata->parse_buff);
        } else if (*p == FIRMATA_END_SYSEX) {
            firmata->parse_command_len = firmata->parse_count + 1;
        } else if (*p & 0x80) {
            firmata->parse_command_len = 1;
            firmata->parse_count = 0;
        }
        if (firmata->parse_count < (int) sizeof(firmata->parse_buff)) {
            firmata->parse_buff[firmata->parse_count] = (uint8_t)(*p);
            firmata->parse_count++;
        }
        if (firmata->parse_count == firmata->parse_command_len) {
            firmata_endParse(firmata);
            firmata->parse_count = 0;
            firmata->parse_command_len = 0;
        }
    }
}

void
firmata_endParse(t_firmata* firmata)
{
    uint8_t cmd = (firmata->parse_buff[0] & 0xF0);
    int pin;

    if (cmd == 0xE0 && firmata->parse_count == 3) {
        int analog_ch = (firmata->parse_buff[0] & 0x0F);
        int analog_val = firmata->parse_buff[1] | (firmata->parse_buff[2] << 7);
        for (pin = 0; pin < 128; pin++) {
            if (firmata->pins[pin].analog_channel == analog_ch) {
                if (pthread_spin_lock(&firmata->lock) != 0) return;
                firmata->pins[pin].value = analog_val;
                if (pthread_spin_unlock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock");
                return;
            }
        }
        return;
    }
    if (cmd == 0x90 && firmata->parse_count == 3) {
        int port_num = (firmata->parse_buff[0] & 0x0F);
        int port_val = firmata->parse_buff[1] | (firmata->parse_buff[2] << 7);
        int pin = port_num * 8;
        int mask;
        for (mask = 1; mask & 0xFF; mask <<= 1, pin++) {
            if (firmata->pins[pin].mode == MODE_INPUT) {
                uint32_t val = (port_val & mask) ? 1 : 0;
                if (pthread_spin_lock(&firmata->lock)) return;
                firmata->pins[pin].value = val;
                if (pthread_spin_unlock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock");
            }
        }
        return;
    }
    if (firmata->parse_buff[0] == FIRMATA_START_SYSEX &&
        firmata->parse_buff[firmata->parse_count - 1] == FIRMATA_END_SYSEX) {
        if (firmata->parse_buff[1] == FIRMATA_REPORT_FIRMWARE) {
            int len = 0;
            int i;
            for (i = 4; i < firmata->parse_count - 2; i += 2) {
                firmata->firmware[len++] =
                (firmata->parse_buff[i] & 0x7F) | ((firmata->parse_buff[i + 1] & 0x7F) << 7);
            }
            firmata->firmware[len++] = '-';
            firmata->firmware[len++] = firmata->parse_buff[2] + '0';
            firmata->firmware[len++] = '.';
            firmata->firmware[len++] = firmata->parse_buff[3] + '0';
            firmata->firmware[len++] = 0;
            syslog(LOG_INFO, "firmata: sketch name:: %s", firmata->firmware);
            // query the board's capabilities only after hearing the
            // REPORT_FIRMWARE message.  For boards that reset when
            // the port open (eg, Arduino with reset=DTR), they are
            // not ready to communicate for some time, so the only
            // way to reliably query their capabilities is to wait
            // until the REPORT_FIRMWARE message is heard.
            char buf[80];
            len = 0;
            buf[len++] = FIRMATA_START_SYSEX;
            buf[len++] = FIRMATA_ANALOG_MAPPING_QUERY; // read analog to pin # info
            buf[len++] = FIRMATA_END_SYSEX;
            buf[len++] = FIRMATA_START_SYSEX;
            buf[len++] = FIRMATA_CAPABILITY_QUERY; // read capabilities
            buf[len++] = FIRMATA_END_SYSEX;
            for (i = 0; i < 16; i++) {
                buf[len++] = 0xC0 | i; // report analog
                buf[len++] = 1;
                buf[len++] = 0xD0 | i; // report digital
                buf[len++] = 1;
            }
            firmata->isReady = 1;
            mraa_uart_write(firmata->uart, buf, len);
        } else if (firmata->parse_buff[1] == FIRMATA_CAPABILITY_RESPONSE) {
            int pin, i, n;
            for (pin = 0; pin < 128; pin++) {
                firmata->pins[pin].supported_modes = 0;
            }
            for (i = 2, n = 0, pin = 0; i < firmata->parse_count; i++) {
                if (firmata->parse_buff[i] == 127) {
                    pin++;
                    n = 0;
                    continue;
                }
                if (n == 0) {
                    // first byte is supported mode
                    firmata->pins[pin].supported_modes |= (1 << firmata->parse_buff[i]);
                }
                n = n ^ 1;
            }
            // send a state query for for every pin with any modes
            for (pin = 0; pin < 128; pin++) {
                char buf[512];
                int len = 0;
                if (firmata->pins[pin].supported_modes) {
                    buf[len++] = FIRMATA_START_SYSEX;
                    buf[len++] = FIRMATA_PIN_STATE_QUERY;
                    buf[len++] = pin;
                    buf[len++] = FIRMATA_END_SYSEX;
                }
                mraa_uart_write(firmata->uart, buf, len);
            }
        } else if (firmata->parse_buff[1] == FIRMATA_ANALOG_MAPPING_RESPONSE) {
            int pin = 0;
            int i;
            for (i = 2; i < firmata->parse_count - 1; i++) {
                firmata->pins[pin].analog_channel = firmata->parse_buff[i];
                pin++;
            }
            return;
        } else if (firmata->parse_buff[1] == FIRMATA_PIN_STATE_RESPONSE && firmata->parse_count >= 6) {
            int pin = firmata->parse_buff[2];
            firmata->pins[pin].mode = firmata->parse_buff[3];
            firmata->pins[pin].value = firmata->parse_buff[4];
            if (firmata->parse_count > 6)
                firmata->pins[pin].value |= (firmata->parse_buff[5] << 7);
            if (firmata->parse_count > 7)
                firmata->pins[pin].value |= (firmata->parse_buff[6] << 14);
        // disable this to check the firmata_devs responses
        } else if (firmata->parse_buff[1] == FIRMATA_I2C_REPLY) {
            int addr = (firmata->parse_buff[2] & 0x7f) | ((firmata->parse_buff[3] & 0x7f) << 7);
            int reg = (firmata->parse_buff[4] & 0x7f) | ((firmata->parse_buff[5] & 0x7f) << 7);
            int i = 6;
            int ii = 0;
            if (pthread_spin_lock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock, skipping i2c msg");
            for (; ii < (firmata->parse_count - 7) / 2; ii++) {
                firmata->i2cmsg[addr][reg+ii] = (firmata->parse_buff[i] & 0x7f) | ((firmata->parse_buff[i+1] & 0x7f) << 7);
                i = i+2;
            }
            if (pthread_spin_unlock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock");
        } else {
            if (firmata->devs != NULL) {
                struct _firmata* devs = firmata->devs[0];
                int i = 0;
                for (; i < firmata->dev_count; i++, devs++) {
                    if (devs != NULL) {
                        if (firmata->parse_buff[1] == devs->feature) {
                            // call func
                            if (devs->isr) {
                                devs->isr(firmata->parse_buff, firmata->parse_count);
                            }
                        }
                    }
                }
            }
        }
        return;
    }
}

void
firmata_initPins(t_firmata* firmata)
{
    int i;

    firmata->parse_count = 0;
    firmata->parse_command_len = 0;
    firmata->isReady = 0;
    for (i = 0; i < 128; i++) {
        firmata->pins[i].mode = 255;
        firmata->pins[i].analog_channel = 127;
        firmata->pins[i].supported_modes = 0;
        firmata->pins[i].value = 0;
    }
}

int
firmata_askFirmware(t_firmata* firmata)
{
    char buf[3];
    int res;

    buf[0] = FIRMATA_START_SYSEX;
    buf[1] = FIRMATA_REPORT_FIRMWARE; // read firmata name & version
    buf[2] = FIRMATA_END_SYSEX;
    res = mraa_uart_write(firmata->uart, buf, 3);
    return (res);
}

int
firmata_pinMode(t_firmata* firmata, int pin, int mode)
{
    int res;
    char buff[4];

    firmata->pins[pin].mode = mode;
    buff[0] = FIRMATA_SET_PIN_MODE;
    buff[1] = pin;
    buff[2] = mode;
    res = mraa_uart_write(firmata->uart, buff, 3);
    return (res);
}

int
firmata_analogWrite(t_firmata* firmata, int pin, int value)
{
    int res;

    char buff[3];
    buff[0] = 0xE0 | pin;
    buff[1] = value & 0x7F;
    buff[2] = (value >> 7) & 0x7F;
    res = mraa_uart_write(firmata->uart, buff, 3);
    return (res);
}

int
firmata_analogRead(t_firmata *firmata, int pin)
{
    int res;
    int value = 1;
    char buff[2];
    buff[0] = FIRMATA_REPORT_ANALOG | pin;
    buff[1] = value;
    res = mraa_uart_write(firmata->uart, buff, 2);
    return res;
}

int
firmata_digitalWrite(t_firmata* firmata, int pin, int value)
{
    int i;
    int res;
    char buff[4];

    if (pin < 0 || pin > 127)
        return (0);
    firmata->pins[pin].value = value;
    int port_num = pin / 8;
    int port_val = 0;
    for (i = 0; i < 8; i++) {
        int p = port_num * 8 + i;
        if (firmata->pins[p].mode == MODE_OUTPUT || firmata->pins[p].mode == MODE_INPUT) {
            if (firmata->pins[p].value) {
                port_val |= (1 << i);
            }
        }
    }
    buff[0] = FIRMATA_DIGITAL_MESSAGE | port_num;
    buff[1] = port_val & 0x7F;
    buff[2] = (port_val >> 7) & 0x7F;
    res = mraa_uart_write(firmata->uart, buff, 3);
    return (res);
}