//Use g++

// https://blog.mbedded.ninja/programming/operating-systems/linux/linux-serial-ports-using-c-cpp/
// C library headers
#include <stdio.h>
#include <string.h>
#include <stdlib.h> // system
#include <iostream>     // cout, cin, streambuf, hex, endl, sgetc, sbumpc
#include <iomanip>      // setw, setfill
#include <fstream>      // fstream
#include <thread>         // std::thread

// Linux headers
#include <fcntl.h> // Contains file controls like O_RDWR
#include <errno.h> // Error integer and strerror() function
#include <termios.h> // Contains POSIX terminal control definitions
#include <unistd.h> // write(), read(), close()

using namespace std;

char msg[256];  // string for serial write
char read_buf [256]; // string for serial read
int en1=0, en2=0; // status of stpper 1 and 2
long cnt=0; // counter of loop (ms)
int nmb=0; // file name number
int keyv=-1;
int keyp=0;
int mde=1;


int intp_stat=0;
void interpret(int ch)
{
    printf("intp: %i %c\n", ch, (char)ch);
    if (ch=='E')
    {
        intp_stat=1;
        return;
    }
    if ((intp_stat==1) && (ch=='1'))
    {
        intp_stat=0;
        en1=2;
        printf("E1 appeared\n");
        return;
    };
    if ((intp_stat==1) && (ch=='2'))
    {
        intp_stat=0;
        en2=2;
        printf("E2 appeared\n");
        return;
    };
}

void key_loop()
{
    // Read single characters from cin.
    std::streambuf *pbuf = cin.rdbuf();
    bool done = false;
    while (!done)
    {
        //cout << "Enter an character (or esc to quit): " << endl;
        char c;
        if (keyp==0) {
		if (pbuf->sgetc() == EOF) done = true;
		c = pbuf->sbumpc();
		//if (c == 0x1b) {
		//    done = true;
		//} else {
		//    cout << "You entered character 0x" << setw(2) << setfill('0') << hex << int(c) << "'" << endl;
		//}
		keyv=c;
		keyp=1;
	} else {
		usleep(1000);
	}
    }
}

void print_usage() {
    printf("Keys:\n");
    printf("  1:   Manual mode to control steppers (see below)\n");
    printf("  2:   Start digitizing (stop with 1 or escape)\n");
    printf("  3:   Reset digitizing filename counter\n");
    printf("  ESC: Exit program\n");
    printf("\n");
    printf("Manual mode (see ESP32 program):\n");
    printf("  Aa/Bb : one frame / back\n");
    printf("  Cc/Dd : one step / back\n");
    printf("  Ee/Ff : continious / back\n");
    printf("  Gg/Hh : one third frame / back\n");
    printf("  Qq    : Quit moving\n");
    printf("  Ww/Xx : decrease / incraese accel\n");
    printf("  Yy/Zz : decrease / incraese speed\n"); 
    printf("\n");
    printf("Starting in mode 1, direct control.\n");
    printf("\n");
}


int main()
{
    print_usage();
    
// single charachter form keyboard read
    struct termios t;
    struct termios t_saved;

    // Set terminal to single character mode.
    tcgetattr(fileno(stdin), &t);
    t_saved = t;
    t.c_lflag &= (~ICANON & ~ECHO);
    t.c_cc[VTIME] = 0;
    t.c_cc[VMIN] = 1;
    if (tcsetattr(fileno(stdin), TCSANOW, &t) < 0)
    {
        perror("Unable to set terminal to single character mode");
        return -1;
    }
    std::thread key_watch (key_loop);

    // Open the serial port. Change device path as needed (currently set to an standard FTDI USB-UART cable type device)
    int serial_port = open("/dev/ttyUSB0", O_RDWR);
    int flags = fcntl(serial_port, F_GETFL, 0);
    fcntl(serial_port, F_SETFL, flags | O_NONBLOCK);

    // Create new termios struct, we call it 'tty' for convention
    struct termios tty;

    // Read in existing settings, and handle any error
    if(tcgetattr(serial_port, &tty) != 0)
    {
        printf("Error %i from tcgetattr: %s\n", errno, strerror(errno));
        return 1;
    }

    tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity (most common)
    tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication (most common)
    tty.c_cflag &= ~CSIZE; // Clear all bits that set the data size
    tty.c_cflag |= CS8; // 8 bits per byte (most common)
    tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control (most common)
    tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines (CLOCAL = 1)

    tty.c_lflag &= ~ICANON;
    tty.c_lflag &= ~ECHO; // Disable echo
    tty.c_lflag &= ~ECHOE; // Disable erasure
    tty.c_lflag &= ~ECHONL; // Disable new-line echo
    tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
    tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
    tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // Disable any special handling of received bytes

    tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
    tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
    // tty.c_oflag &= ~OXTABS; // Prevent conversion of tabs to spaces (NOT PRESENT ON LINUX)
    // tty.c_oflag &= ~ONOEOT; // Prevent removal of C-d chars (0x004) in output (NOT PRESENT ON LINUX)

    tty.c_cc[VTIME] = 10;    // Wait for up to 1s (10 deciseconds), returning as soon as any data is received.
    tty.c_cc[VMIN] = 0;

    // Set in/out baud rate to be 9600
    cfsetispeed(&tty, B115200);
    cfsetospeed(&tty, B115200);

    // Save tty settings, also checking for error
    if (tcsetattr(serial_port, TCSANOW, &tty) != 0)
    {
        printf("Error %i from tcsetattr: %s\n", errno, strerror(errno));
        return 1;
    }

// loop
    bool lp = true;
    while (lp)
    {

        if (keyp>0)
        {
            printf("Key pressed: %c\n", keyv);
            int keyt=keyv;
            if (keyt==0x1b)
            {
                lp=false;   // end (escape)
                keyp=0; // only erase when used here
            }
            if (keyt=='1')
            {
                mde=1;   // control mode
                keyp=0; // only erase when used here
            }
            if (keyt=='2')
            {
                mde=2;   // run mode
                keyp=0; // only erase when used here
            }
            if (keyt=='3')
            {
                nmb=0;   // reset numbering
        keyp=0; // only erase when used here
            }

        }

        if (mde==1)
        {

            memset(msg,0,strlen(msg)); // empty msg
            if (keyp>0) {
                msg[0]=keyv;
                //msg[1]=10;
                msg[1]='\0';
                keyp=0;
                //printf("%li md1 (->) %s, %li\n", cnt, msg, strlen(msg));
            }
            if (strlen(msg)>0)
            {
                printf("%li -> %s\n", cnt, msg);
                write(serial_port, msg, strlen(msg));
                memset(msg,0,strlen(msg)); // empty msg
            }

            memset(&read_buf, '\0', sizeof(read_buf));
            int num_bytes = read(serial_port, &read_buf, sizeof(read_buf));
            if (num_bytes > 0)
            {
                //msg[num_bytes]='\0';
                //printf("%li Read %i bytes. Received message: %s\n", cnt, num_bytes, read_buf);
                printf("%li <- %s (%i)\n", cnt, read_buf, num_bytes);
            }

            usleep(1000);
        } // end mde 1


        if (mde==2)
        {
            if ((en1==0) && (en2==0))
            {
                //if (cnt % 3000 == 0) {
                strcpy(msg, "Ga"); // third turn on frame. Full turn on reel
                en1=1;
                en2=1;
            }

            if (strlen(msg)>0)
            {
                printf("%li -> %s\n", cnt, msg);
                write(serial_port, msg, sizeof(msg));
                memset(msg,0,strlen(msg)); // empty msg
            }

            //memset(&read_buf, '\0', sizeof(read_buf));
            int num_bytes = read(serial_port, &read_buf, sizeof(read_buf));
//    if (num_bytes < 0) {
//      printf("Error reading: %s", strerror(errno));
//      return 1;
//    }

            if (num_bytes > 0)
            {
                //printf("%li Read %i bytes. Received message: %s\n", cnt, num_bytes, read_buf);
                printf("%li <- %s (%i)\n", cnt, read_buf, num_bytes);
                for (int i=0; i<num_bytes; i++)
                {
                    char ch=read_buf[i];
                    interpret((int)ch);
                }
            }

            if ((en1==2) && (en2==2))
            {
                char cmd[250];
                //sprintf(cmd, "fswebcam /dev/video0 -r 1280x1024 --no-banner imgs/tmp%06i.jpg", nmb);
                sprintf(cmd, "./read_pict imgs/tmp%06i.jpg", nmb);
                printf("CMD: %s\n" , cmd);
                nmb++;
                int rv = system(cmd);
                printf("Return value of system: %i\n", rv);
                en1=0;
                en2=0;
            }
            else
            {
                usleep(1000);
            }
        } // end mde 2

        cnt++;
    }
    close(serial_port);

    // Restore terminal mode.
    if (tcsetattr(fileno(stdin), TCSANOW, &t_saved) < 0)
    {
        perror("Unable to restore terminal mode");
        return -1;
    }

    return 0; // success

}