#include <libserialport.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <optional>
#include <thread>
#include <vector>
#include <iomanip>
#include <sstream>

#include <fstream>
#include <boost/process/search_path.hpp>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>
#include <boost/chrono.hpp>  //-lboost_chrono
#include <boost/algorithm/string.hpp> //split 
#include <string>
//g++ libserialport.cpp -std=c++17 -lpthread -lboost_system -lboost_thread -lboost_chrono -lserialport -lrt -pthread -o libserialport
//https://gist.github.com/zekroTJA/00317b41aa69f38090071b6c8065272b
/**
 *  Create asynchronous timers which execute specified
 *  functions in set time interval.
 *  
 *  @param func		Function which sould be executed
 *  @param interval	Interval of time in which function will be executed
 *					(in milliseconds)
 */
class Timer {

public:
	Timer() {}

	Timer(std::function<void(void)> func, const long &interval) {
		m_func = func;
		m_interval = interval;
	}

	/**
	 * Starting the timer.
	 */
	void start() {
		m_running = true;
		m_thread = std::thread([&]() {
			while (m_running) {
				auto delta = std::chrono::steady_clock::now() + std::chrono::milliseconds(m_interval);
				m_func();
				std::this_thread::sleep_until(delta);
			}
		});
		m_thread.detach();
	}

	/*
	 *  Stopping the timer and destroys the thread.
	 */
	void stop() {
		m_running = false;
		m_thread.~thread();
	}

	/*
	 *  Restarts the timer. Needed if you set a new
	 *  timer interval for example.
	 */
	void restart() {
		stop();
		start();
	}
	
	/*
	 *  Check if timer is running.
	 *  
	 *  @returns boolean is running
 	 */
	bool isRunning() { 
		return m_running; 
	}


	/*
	*  Set the method of the timer after
	*  initializing the timer instance.
	*
	*  @returns boolean is running
	*  @return  Timer reference of this
	*/
	Timer *setFunc(std::function<void(void)> func) {
		m_func = func;
		return this;
	}

	/*
	 *  Returns the current set interval in milliseconds.
	 *
	 *  @returns long interval
	 */
	long getInterval() { 
		return m_interval; 
	}

	/*
	*  Set a new interval for the timer in milliseconds.
	*  This change will be valid only after restarting
	*  the timer.
	*
	*  @param interval new interval
	*  @return  Timer reference of this
	*/
	Timer *setInterval(const long &interval) {
		m_interval = interval;
		return this;
	}

	~Timer() {
		stop();
	}
private:
	// Function to be executed fater interval
	std::function<void(void)> m_func;
	// Timer interval in milliseconds
	long m_interval;

	// Thread timer is running into
	std::thread m_thread;
	// Status if timer is running
	bool m_running = false;
};

class SerialClass{
    public:
		SerialClass(){
			std::cout << "SerialClass Started OK." << std::endl; 
		}
		
		~SerialClass(){
			std::cout << "SerialClass Ended ~OK." << std::endl;
			if(port)
				sp_close(port);
		}
		
		bool Start(){
			if(!list_ports()){
				return false;
			}
			mThreadShouldRun = true; 
			mThread = boost::thread(&SerialClass::Run, this);
			return true;
		}

		void Stop(){
			mThreadShouldRun = false;// dont really needed anymore but may be ok
			if(port)
				sp_close(port);
			mThread.interrupt();
			mThread.join();
		}
			
		bool isSerialRunning(){
			return mThreadShouldRun;
		}

		void Run(){	
				//DO Something
				enum sp_return error = sp_get_port_by_name(desired_port.c_str(),&port);
				if (error == SP_OK) {
					error = sp_open(port,(sp_mode)(SP_MODE_READ | SP_MODE_WRITE));
					if (error == SP_OK) {
						sp_set_baudrate(port,115200);
						while(mThreadShouldRun){
							try {
								boost::this_thread::sleep_for(boost::chrono::milliseconds(500));
								int bytes_waiting = sp_input_waiting(port);
								if (bytes_waiting > 0) {
									//printf("Bytes waiting %i\n", bytes_waiting);
									char byte_buff[512];
									int byte_num = 0;
									byte_num = sp_nonblocking_read(port,byte_buff,512);
									
									std::string read_str=std::string(byte_buff);
									//std::cout << "read_str: " << read_str << std::endl;
									size_t exp = read_str.find(exspected);
									if( exp != std::string::npos ){
										is_exspected=true;
										//std::cout << "is_exspected OK" << std::endl;
									}
								}
								fflush(stdout);										
							} catch(boost::thread_interrupted interrupt) {
								std::cout << "boost::thread_interrupted." << std::endl;
								mThreadShouldRun = false;
								break;
							}
						}
					} else {
						printf("Error opening serial device\n");
						mThreadShouldRun = false;
					}
				} else {
					printf("Error finding serial device\n");
					mThreadShouldRun = false;
				}
		}
		
		void set_port(std::string val){
			desired_port=val;
		}
		
		void write(std::string val){
			int ret=sp_nonblocking_write(port,val.c_str(),val.length());
			
			char *error;
			switch (ret) {
				case SP_ERR_ARG:
					std::cout << "serial_write: invalid serial port parameters." << std::endl;
					//return -DEVICE_CONF_ERROR;
				case SP_ERR_FAIL:
					error = sp_last_error_message();
					std::cout << "serial_write: write error: " << sp_last_error_code() << " error: " << error << std::endl;
					sp_free_error_message(error);
					//return -DEVICE_CONN_ERROR;	
			}
		}
		
		void write(std::string val,std::string exspected_str){
			is_exspected=false;
			exspected=exspected_str;
			int ret=sp_nonblocking_write(port,val.c_str(),val.length());
			
			char *error;
			switch (ret) {
				case SP_ERR_ARG:
					std::cout << "serial_write: invalid serial port parameters." << std::endl;
					//return -DEVICE_CONF_ERROR;
				case SP_ERR_FAIL:
					error = sp_last_error_message();
					std::cout << "serial_write: write error: " << sp_last_error_code() << " error: " << error << std::endl;
					sp_free_error_message(error);
					//return -DEVICE_CONN_ERROR;	
			}
		}
		
		bool get_is_exspected(){
			return  is_exspected;
		}
    private:
        boost::thread mThread;
        bool mThreadShouldRun;
		
		std::string desired_port;
		struct sp_port *port=NULL;
		
		std::string exspected;
		bool is_exspected;
		
		bool list_ports() {
			int i;
			bool found=false;
			struct sp_port **ports;

			enum sp_return error = sp_list_ports(&ports);
			if (error == SP_OK) {
				for (i = 0; ports[i]; i++) {
					printf("Found port: '%s'\n", sp_get_port_name(ports[i]));
					if( desired_port == std::string(sp_get_port_name(ports[i]))){
						found=true;	
						break;
					}
				}
				sp_free_port_list(ports);
			} else {
				printf("No serial devices detected\n");
			}
			printf("\n");
			return found;
		}
};

int main(int argc, char **argv){	
	char letter;
	Timer t2; 
	int time_out;
	int iGoUp;
	SerialClass *serial=NULL;	
	serial=new SerialClass();
	serial->set_port("/dev/ttyUSB2");
	if( !serial->Start() ){
		std::cout << "error findig port? " << std::endl;
		letter = 'x';
	}
	
	std::cout << "Started OK" << std::endl;
	while(1) {
		scanf("  %c", &letter );
		if( letter == 'x' ){
			break;
		}else
		if( letter == 'a' ){
			if( serial->isSerialRunning() ){
				std::string send_val="AT\r";
				std::string expected="OK";
				//std::cout << "send_val: " << send_val << std::endl;
				serial->write(send_val,expected);
				
				iGoUp = 0;
				time_out=3;//3sec

				t2.setFunc([&](){
					if (++iGoUp > time_out ){
						t2.stop();
						std::cout << "Time OUT????" << std::endl;
					}else
					if (serial->get_is_exspected()){
						t2.stop();
						std::cout << "Hurray: expected: " << expected << std::endl;
					}
				})
				->setInterval(1000)
				->start();
			}
		}
	}
	
	if( serial->isSerialRunning() ){
		serial->Stop();
		delete serial;
	}
	serial=NULL;  
	return 0;
}