/*
* TURTLE CONTROLLER TEST - 02-06-2021
*
* Compile and run with... 
*
* gcc turtle_controller_test.c -o turtle_controller_test; ./turtle_controller_test
*
*/

#include <stdio.h>      // standard input / output functions
#include <stdlib.h>
#include <string.h>     // string function definitions
#include <unistd.h>     // UNIX standard function definitions
#include <fcntl.h>      // File control definitions
#include <termios.h>    // POSIX terminal control definitions
#include <sys/ioctl.h>  // ioctl() call defenitions

// Define USB serial device for...

// Linux
#define SERIAL_DEVICE	"/dev/ttyUSB0"

// Mac...
//#define SERIAL_DEVICE	"/dev/cu.usbserial-ABSCEZRW"



    void wait_for_done_byte( int usb_serial ){

        tcdrain( usb_serial );    // wait for data

        unsigned char buf[20];

        read( usb_serial, buf, sizeof(buf) - 1 );

        unsigned int rx_byte = buf[0];

        printf( "0x%x COMPLETE\r\n", rx_byte );
    }

    void command_action( int usb_serial, int *cmd, int repeats ){ 

        for( int i=0; i<repeats; i++ ){
            write( usb_serial, cmd, 1 );
        }

        wait_for_done_byte( usb_serial );       
    }

    void command_direction( int usb_serial, int *cmd, int param ){
            
        write( usb_serial, cmd, 1 );
        write( usb_serial, &param, 1 );

        wait_for_done_byte( usb_serial );
    }


void main(){

    int usb_serial = open( SERIAL_DEVICE, O_RDWR| O_NOCTTY );

    struct termios tty;

    memset( &tty, 0, sizeof tty );

    if( tcgetattr ( usb_serial, &tty ) != 0 ){
        printf("Error: could not open serial port!\n\n");
        exit(1);
    }
    
    /* set baud rate */
    cfsetospeed( &tty, (speed_t)B4800 );
    cfsetispeed( &tty, (speed_t)B4800 );

    /* port settings */
    tty.c_cflag     &=  ~PARENB;        // make 8n1
    tty.c_cflag     &=  ~CSTOPB;        // only 1 stop bit
    tty.c_cflag     &=  ~CSIZE;         // clear all size  bits
    tty.c_cflag     |=  CS8;            // 8 bits
    //tty.c_cflag     |=  CRTSCTS;      // enable flow control
    tty.c_cflag     &=  ~CRTSCTS;       // disable flow control
    tty.c_cc[VMIN]   =  1;              // read doesn't block
    tty.c_cc[VTIME]  =  5;              // 0.5 seconds read timeout
    tty.c_cflag     |= CREAD | CLOCAL;  // turn on read

    /* setup for non-canonical mode */
    tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
    tty.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
    tty.c_oflag &= ~OPOST;

    /* fetch bytes as they become available */
    tty.c_cc[VMIN] = 1;
    tty.c_cc[VTIME] = 1;

    cfmakeraw( &tty );    // make raw

    tcflush( usb_serial, TCIFLUSH );

    if( tcsetattr ( usb_serial, TCSANOW, &tty ) != 0 ){
        perror("Error: setting serial port attribute!\n\n");
        exit(1);
    }

    /* control commands */
    int byt_mode = 0xFB;  // tx twice to enter mode
    int cmd_mode = 0xFD;  // tx twice to enter mode
    int cmd_fw   = 0x09;
    int cmd_bk   = 0x06;
    int cmd_lt   = 0x05;
    int cmd_rt   = 0x0A;
    int cmd_pu   = 0x20;    
    int cmd_pd   = 0x10;

    printf("TURTLE CONTROLLER TEST\r\n");

    command_action( usb_serial, &cmd_mode, 2 );

    command_action( usb_serial, &cmd_pd, 1 );

    command_direction( usb_serial, &cmd_fw, 20 );

    command_direction( usb_serial, &cmd_bk, 20 );

    command_direction( usb_serial, &cmd_lt, 90 );

    command_direction( usb_serial, &cmd_rt, 90 );

    command_action( usb_serial, &cmd_pu, 1 );

    close( usb_serial );

    printf("TEST COMPLETE\r\n");
}