#include "inc/LaserRangeFinder.h"

/** Initialize the laser range finder. */
void lrf_init() {
    //char port[] = "/dev/????????";
    char port[100];

    //FILE* fp = popen("dmesg | grep -A 3 \"Arduino Uno\"  | grep -o \"ttyACM.\" | tail -n 1","r");
    FILE* fp = popen("ls -d /dev/serial/by-id/* | grep \"Arduino\" | tr -d \"\n\"", "r");
    //fgets(port+5, sizeof(port)-6, fp);
    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 (uart_filestream == -1)
        return -1;

    //empty buffer
    uart_flush();

    //initialize arrays
    int i;
    char bytes[6];
    for (i = 0; i < 6; i++) {
        bytes[i] = 0;
    }
    int n_read = 0;

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

    int n = 0;
    int d;

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

        if(n_read <= 0)
            return -1;

        for (i = 0; i < n_read; i++) {
            if (bytes[i] != 0xd && bytes[i] != 0xa) {
                value[n++] = bytes[i];
            } else if (atoi(value) != 0) {
                d = atoi(value);
                return d;
            }
        }
    }
}