// © Copyright The University of New South Wales 2018
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program 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/.
//
// adapted from firmata_test
// https://github.com/firmata/firmata_test
// Copyright 2010, Paul Stoffregen (paul@pjrc.com)
// with permission to redistribute and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.

#include "firmatalib.h"
#include <pthread.h>
#include <unistd.h>

// Internals
void firmataclient_domessage(board_t *board);
void firmataclient_parse(board_t *board, const uint8_t *buf, long len);
int  firmataclient_initdata(board_t *board);
void *firmataclient_looplistener(void* data);



// Blink the on-board LED 3 times.
int firmataclient_blink(board_t *board) {
    
    if(board == NULL)  return -1;
    
    // store current mode (in order to reverse it afterwards)
    int current_mode = board->pin_info[LED_BUILTIN].mode;
    firmataclient_changemode(board, LED_BUILTIN, MODE_OUTPUT);
    
    // blink 3 times
    for (int i=0; i<3; ++i) {
        // turn the LED on
        firmataclient_senddigitalvalue(board, LED_BUILTIN, 1);
        //sleep(1);                  // in seconds
        for(long j=0; j<50000000; ++j);
        // turn the LED off
        firmataclient_senddigitalvalue(board, LED_BUILTIN, 0);
        //sleep(1);                  // in seconds
        for(long j=0; j<50000000; ++j);
    }
    
    // reverse the mode change
    firmataclient_changemode(board, LED_BUILTIN, current_mode);
    
    return 0;
}

// Returns the Firmata version currently on the board.
char *firmataclient_getfirmware(board_t *board) {
    if (board == NULL) return "";
    return board->firmata_name;
}

// Opens a communiction port to the board. Returns 0 on success, -1 otherwise.
int firmataclient_initport(board_t *board, char *name, int baud)
{
    if (board == NULL || name == NULL) return -1;
    
    serial_close(board->port);
    board->isBoardReady = false;
    firmataclient_initdata(board);
    serial_open(board->port, name);

    if(serial_open(board->port, name)) {
        char *msg = board->port? board->port->error_msg : "";
        sprintf(board->err_msg, "firmataclient_initport: error opening port %s.\n%s", name, msg);
        return -1;
    }
    
    if (serial_setbaud(board->port, baud)) {
        sprintf(board->err_msg, "firmataclient_initport: error setting baud rate for port %s.", name);
        return -1;
    }

    /*
         The startup strategy is to open the port and immediately
         send the REPORT_FIRMWARE message.  When we receive the
         firmware name reply, then we know the board is ready to
         communicate.
         
         For boards like Arduino which use DTR to reset, they may
         reboot the moment the port opens.  They will not hear this
         REPORT_FIRMWARE message, but when they finish booting up
         they will send the firmware message.
         
         For boards that do not reboot when the port opens, they
         will hear this REPORT_FIRMWARE request and send the
         response.  If this REPORT_FIRMWARE request isn't sent,
         these boards will not automatically send this info.
         
         Arduino boards that reboot on DTR will act like a board
         that does not reboot, if DTR is not raised when the
         port opens.  This program attempts to avoid raising
         DTR on windows.  (is this possible on Linux and Mac OS-X?)
         
         Either way, when we hear the REPORT_FIRMWARE reply, we
         know the board is alive and ready to communicate.

    */
    
    firmataclient_requestfirmware(board);
    
    sprintf(board->err_msg, "");
    return 0;
}


// Sends a firmware query to the board connected.
void firmataclient_requestfirmware(board_t *board) {
   
    if (board == NULL) return;
    
    uint8_t buf[3];
    buf[0] = START_SYSEX;
    buf[1] = REPORT_FIRMWARE; // read firmata name & version
    buf[2] = END_SYSEX;
    long r = serial_write(board->port, buf, 3);
    if(r > 0){
        board->tx_count += 3;
    }
    else {
        sprintf(board->err_msg, "firmataclient_requestfirmware: error writing to board\n");
        return;
    }
}

// sends a new value update to the board.
void firmataclient_sendnewvalue(board_t *board, int pin, int newValue)
{
    if (board == NULL) return;
    if (pin < 0 || pin >= TOTAL_PINS) return;
    
    board->pin_info[pin].value = newValue;
    
    if (pin <= 15 && newValue <= 16383) {
        uint8_t buf[3];
        buf[0] = 0xE0 | pin;
        buf[1] = newValue & 0x7F;
        buf[2] = (newValue >> 7) & 0x7F;
        serial_write(board->port, buf, 3);
        board->tx_count += 3;
    } else {
        uint8_t buf[12];
        int len=4;
        buf[0] = 0xF0;
        buf[1] = 0x6F;
        buf[2] = pin;
        buf[3] = newValue & 0x7F;
        if (newValue > 0x00000080) buf[len++] = (newValue >>  7) & 0x7F;
        if (newValue > 0x00004000) buf[len++] = (newValue >> 14) & 0x7F;
        if (newValue > 0x00200000) buf[len++] = (newValue >> 21) & 0x7F;
        if (newValue > 0x10000000) buf[len++] = (newValue >> 28) & 0x7F;
        buf[len++] = 0xF7;
        serial_write(board->port, buf, len);
        board->tx_count += len;
    }
}

// sends a new mode update to the board.
void firmataclient_changemode(board_t *board, int pin, int newMode)
{
    if (board == NULL) return;
    if (pin < 0 || pin > TOTAL_PINS-1) return;
    
    //printf("Mode Change, pin=%d, %d\n", pin, newMode);
    if (newMode != board->pin_info[pin].mode) {
        // send the mode change message
        uint8_t buf[4];
        buf[0] = 0xF4;
        buf[1] = pin;
        buf[2] = newMode;
        serial_write(board->port, buf, 3);
        board->tx_count += 3;
        board->pin_info[pin].mode = newMode;
        board->pin_info[pin].value = 0;
        //firmataclient_updatestatus(board);
    }
}

// Sends a new digital value pin update to the board.
void firmataclient_senddigitalvalue(board_t *board, int pin, int newValue)
{
    if (board == NULL) return;
    if (pin < 0 || pin >= TOTAL_PINS) return;
    
    board->pin_info[pin].value = newValue;
    int port_num = pin / 8;
    int port_val = 0;
    for (int i=0; i<8; i++) {
        int p = port_num * 8 + i;
        if (board->pin_info[p].mode == MODE_OUTPUT || board->pin_info[p].mode == MODE_PULLUP) {
            if (board->pin_info[p].value) port_val |= (1<<i);
        }
    }
    uint8_t buf[3];
    buf[0] = 0x90 | port_num;
    buf[1] = port_val & 0x7F;
    buf[2] = (port_val >> 7) & 0x7F;
    serial_write(board->port, buf, 3);
    board->tx_count += 3;
}

/*
void firmataclient_setdigitalvalue(board_t *board, int pin, int newValue)
{
    if (board == NULL) return;
    if (pin < 0 || pin >= TOTAL_PINS) return;
    
    board->pin_info[pin].value = newValue;
    int port_num = pin / 8;
    int port_val = 0;
    for (int i=0; i<8; i++) {
        int p = port_num * 8 + i;
        if (board->pin_info[p].mode == MODE_OUTPUT || board->pin_info[p].mode == MODE_INPUT || board->pin_info[p].mode == MODE_PULLUP) {
            if (board->pin_info[p].value) {
                port_val |= (1<<i);
            }
        }
    }
    uint8_t buf[3];
    buf[0] = 0x90 | port_num;
    buf[1] = port_val & 0x7F;
    buf[2] = (port_val >> 7) & 0x7F;
    serial_write(board->port, buf, 3);
    board->tx_count += 3;
    //firmataclient_updatestatus(board);
    
}
*/

// Returns true if the board is ready for communication.
bool firmataclient_isboardready(board_t *board)
{
    if (board == NULL) return false;
    return board->isBoardReady;
}

// Waits (blocks) until the board is ready for communication.
int firmataclient_waitForBoardReady(board_t *board) {
    if (board == NULL) return -1;
    
    while(!board->isBoardReady) {
        if (! board->isOK) return -1;
        firmataclient_onidle(board);
        //sleep(100);
    }
    return 0;
}


// Handles message received from board.
void firmataclient_domessage(board_t *board)
{
    if (board == NULL) return;
    uint8_t cmd = (board->parse_buf[0] & 0xF0);
    
    // analog I/O message
    if (cmd == 0xE0 && board->parse_count == 3) {  //ANALOG_MESSAGE
        int analog_ch = (board->parse_buf[0] & 0x0F);
        int analog_val = board->parse_buf[1] | (board->parse_buf[2] << 7);
        for (int pin=0; pin<128; pin++) {
            if (board->pin_info[pin].analog_channel == analog_ch) {
                board->pin_info[pin].value = analog_val;
                //printf("pin %d is A%d = %d\n", pin, analog_ch, analog_val); //debugging
                return;
            }
        }
        return;
    }
    
    // digital I/O message
    if (cmd == 0x90 && board->parse_count == 3) {  // DIGITAL_MESSAGE
        int port_num = (board->parse_buf[0] & 0x0F);
        int port_val = board->parse_buf[1] | (board->parse_buf[2] << 7);
        int pin = port_num * 8;
        //printf("pin= %d port_num = %d, port_val = %d\n", pin, port_num, port_val); //debugging
        for (int mask=1; mask & 0xFF; mask <<= 1, pin++) {
            if (board->pin_info[pin].mode == MODE_INPUT || board->pin_info[pin].mode == MODE_PULLUP) {
                uint32_t val = (port_val & mask) ? 1 : 0;
                if (board->pin_info[pin].value != val) {
                    //printf("pin %d is %d\n", pin, val); //debugging
                    board->pin_info[pin].value = val;
                }
            }
        }
        return;
    }
    
    // Sysex message
    if (board->parse_buf[0] == START_SYSEX && board->parse_buf[board->parse_count-1] == END_SYSEX) {
        // Sysex message
        if (board->parse_buf[1] == REPORT_FIRMWARE) {
            int len=0;
            for (int i=4; i < board->parse_count-2; i+=2) {
                board->firmata_name[len++] = (board->parse_buf[i] & 0x7F)
                | ((board->parse_buf[i+1] & 0x7F) << 7);
            }
            board->firmata_name[len++] = '-';
            board->firmata_name[len++] = board->parse_buf[2] + '0';
            board->firmata_name[len++] = '.';
            board->firmata_name[len++] = board->parse_buf[3] + '0';
            board->firmata_name[len++] = 0;
            //printf("firmata name is %s\n", firmata_name);
            
            // 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.
            
            uint8_t buf[80];
            len=0;
            buf[len++] = START_SYSEX;
            buf[len++] = ANALOG_MAPPING_QUERY; // read analog to pin # info
            buf[len++] = END_SYSEX;
            buf[len++] = START_SYSEX;
            buf[len++] = CAPABILITY_QUERY; // read capabilities
            buf[len++] = END_SYSEX;
            for (int i=0; i<16; i++) {
                buf[len++] = 0xC0 | i;  // report analog REPORT_ANALOG
                buf[len++] = 1;
                buf[len++] = 0xD0 | i;  // report digital REPORT_DIGITAL
                buf[len++] = 1;
            }
            serial_write(board->port, buf, len);
            board->tx_count += len;
            board->tx_count += 3;
            
        } else if (board->parse_buf[1] == CAPABILITY_RESPONSE) {
            int pin, i, n;
            for (pin=0; pin < 128; pin++) {
                board->pin_info[pin].supported_modes = 0;
            }
            for (i=2, n=0, pin=0; i<board->parse_count; i++) {
                if (board->parse_buf[i] == 127) { // default mode (off)
                    pin++;
                    n = 0;
                    continue;
                }
                if (n == 0) {
                    // first byte is supported mode
                    board->pin_info[pin].supported_modes |= (1<<board->parse_buf[i]);
                }
                n = n ^ 1;
            }
            
            /* commented out the state query so that the pin's initial values on board will be discarded:
            // send a state query for every pin with any modes
            for (pin=0; pin < 128; pin++) {
                uint8_t buf[512];
                int len=0;
                if (pin_info[pin].supported_modes) {
                    buf[len++] = START_SYSEX;
                    buf[len++] = PIN_STATE_QUERY;
                    buf[len++] = pin;
                    buf[len++] = END_SYSEX;
                }
                serial_write(buf, len);
                tx_count += len;
            } end commenting out */
            
            // note that the board is now ready for communication
            // after verfying board's pins capabilities
            board->isBoardReady = true;
            
        } else if (board->parse_buf[1] == ANALOG_MAPPING_RESPONSE) {
            int pin=0;
            for (int i=2; i<board->parse_count-1; i++) {
                board->pin_info[pin].analog_channel = board->parse_buf[i];
                pin++;
            }
            return;
            
        } else if (board->parse_buf[1] == PIN_STATE_RESPONSE && board->parse_count >= 6) {
            int pin = board->parse_buf[2];
            board->pin_info[pin].mode = board->parse_buf[3];
            board->pin_info[pin].value = board->parse_buf[4];
            if (board->parse_count > 6) board->pin_info[pin].value |= (board->parse_buf[5] << 7);
            if (board->parse_count > 7) board->pin_info[pin].value |= (board->parse_buf[6] << 14);
            //printf("pin: %d value: %u", pin, pin_info[pin].value);
            //printSupportedModes(pin);
            
        } else if (board->parse_buf[1] == PULSE_IN && board->parse_count >= 5) {
            int pin = board->parse_buf[2];
            int val = board->parse_buf[3];
            if (board->parse_count > 5) val |= (board->parse_buf[4] << 7);
            if (board->parse_count > 6) val |= (board->parse_buf[5] << 14);
            if (board->parse_count > 7) val |= (board->parse_buf[6] << 21);
            if (board->parse_count > 8) val |= (board->parse_buf[7] << 28);
            //todo?
            //board->pin_info[pin].mode = PULSE_IN;
            board->pin_info[pin].value = val;            
        }

        return;
    }
}


void delayMicroseconds(int t);
long pulseIn(int Echo_pin, bool opp_observable_cond);

void delayMicroseconds(int t) { }
long pulseIn(int Echo_pin, bool opp_observable_cond) {
    return 0;
}

long firmataclient_pulse_duration(board_t *board, int Trig_pin, int Echo_pin, bool observe) {   // Ultrasonic::Timing()

    bool observable_cond = observe;         //LOW
    bool opp_observable_cond = !observe;    //HIGH
    
    firmataclient_senddigitalvalue(board, Trig_pin, observable_cond);    //LOW
    delayMicroseconds(2);
    firmataclient_senddigitalvalue(board, Trig_pin, opp_observable_cond);    //HIGH
    delayMicroseconds(10);
    firmataclient_senddigitalvalue(board, Trig_pin, observable_cond);    //LOW
    long duration = pulseIn(Echo_pin, opp_observable_cond);  //HIGH
    return duration;
}

// Parses the bytes received from serial.
void firmataclient_parse(board_t *board, const uint8_t *buf, long len)
{
    if (board == NULL) return;
    
    const uint8_t *p, *end;
    
    p = buf;
    end = p + len;
    for (p = buf; p < end; p++) {
        uint8_t msn = *p & 0xF0; //0xF0 START_SYSEX;
        if (msn == 0xE0 || msn == 0x90|| *p == 0xF9) {// 0xE0 ANALOG_MESSAGE; 0x90 DIGITAL_MESSAGE; 0xF9 REPORT_VERSION
            board->parse_command_len = 3;
            board->parse_count = 0;
        } else if (msn == 0xC0 || msn == 0xD0) { //0xC0 REPORT_ANALOG; 0xD0 REPORT_DIGITAL
            board->parse_command_len = 2;
            board->parse_count = 0;
        } else if (*p == START_SYSEX) {
            board->parse_count = 0;
            board->parse_command_len = sizeof(board->parse_buf);
        } else if (*p == END_SYSEX) {
            board->parse_command_len = board->parse_count + 1;
        } else if (*p & 0x80) {
            board->parse_command_len = 1;
            board->parse_count = 0;
        }
        if (board->parse_count < (int)sizeof(board->parse_buf)) {
            board->parse_buf[board->parse_count++] = *p;
        }
        if (board->parse_count == board->parse_command_len) {
            firmataclient_domessage(board);
            board->parse_count = board->parse_command_len = 0;
        }
    }
}

// Listen for incoming bytes from serial.
void firmataclient_onidle(board_t *board)
{
    if (board == NULL) return;
    
    uint8_t buf[1024];
    long r;
    
    r = serial_inputwait(board->port, 40);
    if (r > 0) {
        r = serial_read(board->port, buf, sizeof(buf));
        if (r < 0) {
            // error
            board->isOK = false;
            return;
        }
        if (r > 0) {
            // parse
            board->rx_count += r;
            firmataclient_parse(board, buf, r);
            //firmataclient_updatestatus(board);
        }
    } else if (r < 0) {
        board->isOK = false;
    }
}

// Resets all fields.
int firmataclient_initdata(board_t *board)
{
    if (board == NULL) return -1;
    
    board->port = (serial_t *)malloc(sizeof(serial_t));
    if(board->port == NULL) {
        sprintf(board->err_msg, "firmataclient_initdata: couldn't allocate new memory");
        return -1;
    }
    
    for (int i=0; i < 128; i++) {
        board->pin_info[i].mode = 255;
        board->pin_info[i].analog_channel = 127;
        board->pin_info[i].supported_modes = 0;
        board->pin_info[i].value = 0;
    }
    
    board->tx_count = board->rx_count = 0;
    board->parse_count = 0;
    board->parse_command_len = 0;
    board->firmata_name[0] = '\0';
    board->isOK = true;
    board->isBoardReady = false;
    board->threadlistening = false;
    board->err_msg[0] = '\0';
    
    return 0;
    
}

int firmataclient_closeport(board_t *board) {
    
    if (board == NULL) return 0;
    
    if(firmataclient_destroylisteningthread(board)) {
        char * msg = board->port? board->port->error_msg : "";
        sprintf(board->err_msg, "firmataclient_destroylisteningthread: %s", msg);
        return -1;
    }

    serial_close(board->port);
    //free(board->port); //causing NULL pointer ERROR!!! do not free port.
    // firmataclient_initdata(board);
    return 0;
}

// Reads pin values from the board.
void firmataclient_getinputvalues(board_t *board, long *indexArray, long *valuesArray, int pinsCount) {
    if (board == NULL) return;
    if(pinsCount >= TOTAL_PINS) return;
    
    for(int i=0; i<pinsCount; ++i) {
        if(indexArray[i] < TOTAL_PINS)
            valuesArray[i] = board->pin_info[indexArray[i]].value;
    }
}

// Starts an infinite listening thread to recieve board messages.
int firmataclient_launchtlisteninghread(board_t *board)
{
    if (board == NULL) return false;
    
    // Create the thread using POSIX routines.
    pthread_t       posixThreadID;
    int             returnVal;
    
    returnVal = pthread_attr_init(&(board->attr));
    if(returnVal) {
        strcpy(board->err_msg, "err in pthread_attr_init");
        return returnVal;
    }
    returnVal = pthread_attr_setdetachstate(&(board->attr), PTHREAD_CREATE_DETACHED);
    if(returnVal) {
        strcpy(board->err_msg, "err in pthread_attr_setdetachstate");
        return returnVal;
    }
    
    // caution: listening thread expects a pointer to struct board_t as input
    int threadError = pthread_create(&posixThreadID, &(board->attr), &firmataclient_looplistener, board);
    if (threadError)
    {
        sprintf(board->err_msg, "err in pthread_create");
        return threadError;
    }
    
    board->threadlistening = true;
    return 0;
        
}

// Stops listening for incoming board messages (kill the listening thread).
int firmataclient_destroylisteningthread(board_t *board)
{
    if (board == NULL) return 0;
    if( ! board->threadlistening) return 0;
    
    int returnVal = pthread_attr_destroy(&(board->attr));
    if (returnVal != 0)
    {
        sprintf(board->err_msg, "error killing listening thread");
        printf("error killing listening thread\n");
        return -1;
    }
    
    board->threadlistening = false;
    return 0;
}

// continuously listen to incoming board messages
// and update the pin_info representation of the board pins
// caution: this thread expects a pointer to struct board_t as input
void* firmataclient_looplistener(void* data)
{
    if (data == NULL) return NULL;
    
    while(true) {
        firmataclient_onidle((board_t *)data);
    }
}



