//
// Created by nock on 2019-04-25.
//

#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include "serial_util.h"

#define DEFAULT_SPEED B115200

static speed_t speed_from_string(char const *s) {
    if (!s) {
        return DEFAULT_SPEED;
    }
    errno = 0;
    long speed = strtol(s, NULL, 10);
    if (errno) {
        return DEFAULT_SPEED;
    }
    switch (speed) {
    case 9600:
        return B9600;
    case 115200:
        return B115200;
    case 230400:
        return B230400;
    case 460800:
        return B460800;
    case 921600:
        return B921600;
    default:
        return B115200;
    }
}

void serial_flush(int serial_fd) {
    tcflush(serial_fd, TCIOFLUSH);
}

int serial_init(char const *serial_port, char const *speed) {
    int serial_fd = open(serial_port, O_RDWR | O_NOCTTY | O_NDELAY);
    if (serial_fd < 0) {
        fprintf(stderr, "Unable to open %s: %s\n", serial_port, strerror(errno));
        return 1;
    }
    //printf("Opened %s\n", serial_port);

    struct termios t_options;

    if (tcgetattr(serial_fd, &t_options) < 0) {
        fprintf(stderr, "Failed to get termios attrs.\n");
    }

    if (cfsetspeed(&t_options, speed_from_string(speed)) < 0) {
        fprintf(stderr, "Failed to set port speed.\n");
        return 2;
    }

    t_options.c_lflag &= ~(ICANON | ECHO | ECHOE);
    t_options.c_oflag &= ~OPOST;
    if (tcsetattr(serial_fd, TCSANOW, &t_options) < 0){
        fprintf(stderr, "Failed to set termios attrs.\n");
    }
    serial_flush(serial_fd);
    return serial_fd;
}