#include "inc/LaserRangeFinder.h"

int timeout;

/** Initialize the laser range finder. */
void lrf_init() {
    timeout = 0;

    /*
    char port[] = "/dev/????????";
    FILE* fp = popen("dmesg | grep -A 3 \"Arduino Uno\"  | grep -o \"ttyACM.\" | tail -n 1","r");
    fgets(port+5, sizeof(port)-6, fp);
    */

    char port[100];
    FILE* fp = popen("ls -d /dev/serial/by-id/* | grep \"Arduino\" | tr -d \"\n\"", "r");
    fgets(port, sizeof(port), fp);

    printf("lrf port=%s\n", port);
    pclose(fp);

    uart_setup(port);
}

/** Terminate the session. */
void lrf_term() {
    uart_close();
}

/** Checking for noVal. */
GEN_TYPE_boolean lrf_is_noval() {
    return false;
}

/** Get one distance measurement from the laser range finder. */
GEN_TYPE_int lrf_read() {

    if(timeout>1000)
        lrf_init();

    if (uart_filestream == -1) {
        timeout++;
        printf("-1: no filestream\n");
        return -1;
    }

    //empty buffer
    uart_flush();

    //initialize arrays
    int sizeB = 10;
    int sizeV = 4;

    int i;
    char bytes[sizeB];
    for (i = 0; i < sizeB; i++) {
        bytes[i] = 0;
    }
    int n_read = 0;

    char value[sizeV];
    for (i = 0; i < sizeV; i++) {
        value[i] = 0;
    }

    int n = 0;
    int d;

    //receive values
    while (1) {
        n_read = uart_receive(bytes, sizeB, 0);

        if(n_read <= 0) {
            timeout++;
            printf("-1: n_read=%d\n",n_read);
            return -1;
        }

        for (i = 0; i < n_read; i++) {
            if (bytes[i] != 0xd && bytes[i] != 0xa) {
                if(n < sizeV)
                    value[n++] = bytes[i];
                else {
                    timeout++;
                    printf("-1: too many bytes\n");
                    return -1;
                }
            } else {
                d = atoi(value);
                printf("%d\n",d);
                timeout = 0;
                return d;
            }
        }
    }
}