#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>
#include <optional>
#include <thread>
#include <vector>
//#include "utils.hpp"
#include <iomanip>
#include <sstream>
#include <simpleble/SimpleBLE.h>

#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++ ego.cpp utils.cpp -std=c++17 -lpthread -lboost_system -lboost_thread -lboost_chrono -lsimpleble -lsimpleble-c -pthread -o ego `pkg-config --libs --cflags dbus-1`

#include <mutex>

void print_byte_array(const SimpleBLE::ByteArray& bytes) {
    for (auto b : bytes) {
        std::cout << std::hex << std::setfill('0') << std::setw(2) << (uint32_t)((uint8_t)b) << " ";
    }
    std::cout << std::endl;
}

std::string byte_array_to_string(const SimpleBLE::ByteArray& bytes) {
	std::stringstream ss;
  
    for (auto b : bytes) {
        //ss << std::hex << std::setfill('0') << std::setw(2) << (uint32_t)((uint8_t)b) << " ";
		ss << std::hex << std::setfill('0') << std::setw(2) << (uint32_t)((uint8_t)b);
    }
	std::string  hex(ss.str());
	int len = hex.length();
	std::string newString;
	for(int i=0; i< len; i+=2){
		std::string byte = hex.substr(i,2);
		char chr = (char) (int)strtol(byte.c_str(), NULL, 16);
		newString.push_back(chr);
	}

	return newString;
}

/*
std::optional<std::size_t> getUserInputInt(const std::string& line, std::size_t max) {
    std::size_t ret;

    while (!std::cin.eof()) {
        std::cout << line << " (0-" << max << "): ";
        std::cin >> ret;

        if (!std::cin) {
            return {};
        }

        if (ret <= max) {
            return ret;
        }
    }
    return {};
}*/

std::optional<SimpleBLE::Adapter> getAdapter() {
    if (!SimpleBLE::Adapter::bluetooth_enabled()) {
        std::cout << "Bluetooth is not enabled!" << std::endl;
        return {};
    }

    auto adapter_list = SimpleBLE::Adapter::get_adapters();

    // no adapter found
    if (adapter_list.empty()) {
        std::cerr << "No adapter was found." << std::endl;
        return {};
    }

    // only one found, returning directly
    //if (adapter_list.size() == 1) {
        auto adapter = adapter_list.at(0);
        std::cout << "Using adapter: " << adapter.identifier() << " [" << adapter.address() << "]" << std::endl;
        return adapter;
    //}
	/*
    // multiple adapters found, ask user to select one
    std::cout << "Available adapters:" << std::endl;
    int i = 0;
    for (auto& adapter : adapter_list) {
        std::cout << "[" << i++ << "] " << adapter.identifier() << " [" << adapter.address() << "]" << std::endl;
    }

    auto adapter_selection = getUserInputInt("Select an adapter", adapter_list.size() - 1);

    if (!adapter_selection.has_value()) {
        return {};
    }

    return adapter_list[adapter_selection.value()];*/
}

class ScanClass{
    public:
	ScanClass(){
		peripheral_identifier_to_find="NOB";
		peripheral_address_to_find="NOB";
		uuid_read_to_find="NOB";
		uuid_write_to_find="NOB";
		write_str="";
		time_to_scan=5000;
		std::cout << "ScanClass Started OK." << std::endl; 
	}
	
	~ScanClass(){
		std::cout << "ScanClass Ended ~OK." << std::endl;
	}
	
		void Start(){
			mThreadShouldRun = true; 
			mThread = boost::thread(&ScanClass::Run, this);
		}

		void Stop(){
			mThreadShouldRun = false;// dont really needed anymore but may be ok
			if( peripheral.is_connected()){
				std::cout << "peripheral disconnect" << std::endl;
				if( uuid_write_idx != -1 )
					peripheral.unsubscribe(uuids[uuid_write_idx].first, uuids[uuid_write_idx].second);
				peripheral.disconnect();
			}
			mThread.interrupt();
			mThread.join();
		}

		void Run(){
			peripheral_identifier="NOB";
			peripheral_address="NOB";
			int uuid_read_idx=-1;
			uuid_write_idx=-1;
			peripheral_idx=-1;
			
					auto adapter_optional = getAdapter();

					if (!adapter_optional.has_value()) {
						//return EXIT_FAILURE;
						//break;
						mThreadShouldRun=false;
						std::cout << "Error adapter_optional has NO value" << std::endl;
						return;
					}

					auto adapter = adapter_optional.value();

					adapter.set_callback_on_scan_found([](SimpleBLE::Peripheral peripheral) {
						std::cout << "Found device: " << peripheral.identifier() << " [" << peripheral.address() << "] " << peripheral.rssi() << " dBm" << std::endl;
					});

					adapter.set_callback_on_scan_updated([](SimpleBLE::Peripheral peripheral) {
						std::cout << "Updated device: " << peripheral.identifier() << " [" << peripheral.address() << "] " << peripheral.rssi() << " dBm" << std::endl;
					});

					adapter.set_callback_on_scan_start([]() { std::cout << "Scan started." << std::endl; });

					adapter.set_callback_on_scan_stop([]() { std::cout << "Scan stopped." << std::endl; });

					// Scan for default 5 seconds.
					adapter.scan_for(time_to_scan);
					
					std::vector<SimpleBLE::Peripheral> peripherals = adapter.scan_get_results();
					//std::cout << "The following devices were found:" << std::endl;
					for (size_t i = 0; i < peripherals.size(); i++) {
						std::string peripheral_address_tem(peripherals[i].address());
						if( peripheral_address_to_find == peripheral_address_tem){
							std::string peripheral_identifier_tem(peripherals[i].identifier());
							peripheral_address=peripheral_address_tem;
							peripheral_identifier=peripheral_identifier_tem;
							dbm_int=peripherals[i].rssi();
							tx_power_int=peripherals[i].tx_power();
							addr_type=peripherals[i].address_type();
							peripheral_idx=i;
						}
						
						/*std::string connectable_string = peripherals[i].is_connectable() ? "Connectable" : "Non-Connectable";
						std::string peripheral_string = peripherals[i].identifier() + " [" + peripherals[i].address() + "] " + std::to_string(peripherals[i].rssi()) + " dBm";						

						std::cout << "[" << i << "] " << peripheral_string << " " << connectable_string << std::endl;

						std::cout << "    Tx Power: " << std::dec << peripherals[i].tx_power() << " dBm" << std::endl;
						std::cout << "    Address Type: " << peripherals[i].address_type() << std::endl;

						std::vector<SimpleBLE::Service> services = peripherals[i].services();
						for (auto& service : services) {
							std::cout << "    Service UUID: " << service.uuid() << std::endl;
							std::cout << "    Service data: ";
							print_byte_array(service.data());
						}

						std::map<uint16_t, SimpleBLE::ByteArray> manufacturer_data = peripherals[i].manufacturer_data();
						for (auto& [manufacturer_id, data] : manufacturer_data) {
							std::cout << "    Manufacturer ID: " << manufacturer_id << std::endl;
							std::cout << "    Manufacturer data: ";
							print_byte_array(data);
						}*/
					}

					std::cout << "Scan complete." << std::endl;
					
					if( get_peripheral_address() != "NOB" && peripheral_idx != -1 ){
						//auto peripheral = peripherals[peripheral_idx];
						//SimpleBLE::Peripheral  
						peripheral = peripherals[peripheral_idx];
											
						//std::cout << "Connecting to " << peripheral.identifier() << " [" << peripheral.address() << "]" << std::endl;
						peripheral.connect();
						
						if( peripheral.is_connected() ){
							std::cout << "Successfully connected" << std::endl;

							for (auto service : peripheral.services()) {
								for (auto characteristic : service.characteristics()) {
									uuids.push_back(std::make_pair(service.uuid(), characteristic.uuid()));
								}
							}

							//std::cout << "The following services and characteristics were found:" << std::endl;
							for (size_t i = 0; i < uuids.size(); i++) {
								//std::cout << "[" << i << "] " << uuids[i].first << " " << uuids[i].second << std::endl;
								std::string uuid_tem(uuids[i].second);
								boost::to_upper(uuid_tem);
								boost::to_upper(uuid_read_to_find);
								boost::to_upper(uuid_write_to_find);
								//std::cout << "compare uuid_tem: " << uuid_tem << " to uuid_read_to_find: "<< uuid_read_to_find << std::endl;
								if( uuid_read_to_find == uuid_tem ){
									uuid_read_idx=i;
								}
								if( uuid_write_to_find == uuid_tem ){
									uuid_write_idx=i;	

									// Subscribe to the characteristic.
									peripheral.notify(uuids[uuid_write_idx].first, uuids[uuid_write_idx].second, [&](SimpleBLE::ByteArray bytes) {
										std::cout << "Received: " << byte_array_to_string(bytes) << std::endl;
										//print_byte_array(bytes);
									});
								}
							}
							/*
							if( uuid_read_to_find != "NOB" ){
								// NOTE: Alternatively, `write_command` can be used to write to a characteristic too.
								// `write_request` is for unacknowledged writes(uanerkendt skriver).
								// `write_command` is for acknowledged writes(anerkendt skriver).
								std::cout << "We have uuid_read_to_find: " << uuid_read_to_find << std::endl;
								if( uuid_read_idx != -1 ){
									if( write_str != "" ){
										std::cout << "We have uuid_read_idx: " << uuid_read_idx << " Sending: 0x01" << std::endl;
										//peripheral.write_request(uuids[uuid_read_idx].first, uuids[uuid_read_idx].second, "0x01");
										peripheral.write_command(uuids[uuid_read_idx].first, uuids[uuid_read_idx].second, "0x01");
										boost::this_thread::sleep_for(boost::chrono::seconds(1));
										peripheral.write_command(uuids[uuid_read_idx].first, uuids[uuid_read_idx].second, write_str);
										write_str = "";
									}
								}
							}*/
						}else{
							mThreadShouldRun=false;
							std::cout << "Error Unable to connect blurtooth.????" << std::endl;
							return;							
						}

						//peripheral.disconnect();
					}else{
						std::cout << "Error no peperials found????" << std::endl;
						mThreadShouldRun=false;
						return;
					}
			
			while(mThreadShouldRun){
				//DO Something
				try {

	
					if( peripheral.is_connected()){
						if( uuid_read_to_find != "NOB" ){
							// NOTE: Alternatively, `write_command` can be used to write to a characteristic too.
							// `write_request` is for unacknowledged writes(uanerkendt skriver).
							// `write_command` is for acknowledged writes(anerkendt skriver).
							//std::cout << "We have uuid_read_to_find: " << uuid_read_to_find << std::endl;
							if( uuid_read_idx != -1 ){
								if( write_str != "" ){
									std::cout << "Sending: " << write_str << " uuid: " << uuid_read_to_find << std::endl;
									//peripheral.write_request(uuids[uuid_read_idx].first, uuids[uuid_read_idx].second, "0x01");
									//peripheral.write_command(uuids[uuid_read_idx].first, uuids[uuid_read_idx].second, "0x01");
									//boost::this_thread::sleep_for(boost::chrono::seconds(1));
									peripheral.write_command(uuids[uuid_read_idx].first, uuids[uuid_read_idx].second, write_str);
									write_str = "";
								}
							}
						}
						
						if( uuid_write_to_find != "NOB" ){
							// NOTE: Alternatively, `write_command` can be used to write to a characteristic too.
							// `write_request` is for unacknowledged writes(uanerkendt skriver).
							// `write_command` is for acknowledged writes(anerkendt skriver).
							//std::cout << "We have uuid_write_to_find: " << uuid_write_to_find << std::endl;
							if( uuid_write_idx != -1 ){
								//SimpleBLE::ByteArray rx_data = peripheral.read(uuids[uuid_write_idx].first, uuids[uuid_write_idx].second);
								//std::string byt(rx_data, sizeof(rx_data));
								//std::cout << "byt: " << rx_data << " uuid: " << uuid_write_to_find << std::endl;
								/*size_t lenght = sizeof(rx_data) / sizeof(SimpleBLE::ByteArray);
								if( lenght > 1 ){
									std::cout << "lenght: " << lenght << " rx_data: " << rx_data << " uuid: " << uuid_write_to_find << std::endl;
								}*/
								//std::cout << "Characteristic content is: ";
								//print_byte_array(rx_data);
								//std::this_thread::sleep_for(1s);
								//std::cout << "Reading uuid: " << uuid_write_to_find << std::endl;
							}
						}
					}
						
					//mThreadShouldRun=false;
					//break;
					//boost::this_thread::sleep_for(boost::chrono::seconds(15));
					boost::this_thread::sleep_for(boost::chrono::milliseconds(500));
				} catch(boost::thread_interrupted interrupt) {
					std::cout << "boost::thread_interrupted." << std::endl;
					break;
				}
			}
		}
		
		bool isScanRunning(){
			return mThreadShouldRun;
		}
		
		bool peripheral_is_connected(){
			return peripheral.is_connected();
		}
		
		void set_time_to_scan(int val){
			time_to_scan=val;
		}
		
		void set_peripheral_identifier_to_find(std::string val){
			peripheral_identifier_to_find=val;
		}
		
		void set_peripheral_address_to_find(std::string val){
			peripheral_address_to_find=val;
		}
		
		void set_uuid_read_to_find(std::string val){
			uuid_read_to_find=val;
		}
		
		void set_uuid_write_to_find(std::string val){
			uuid_write_to_find=val;
		}
		
		void do_write(std::string val){
			write_str=val;
		}
		
		std::string get_peripheral_address(){
			return peripheral_address;
		}
		
		std::string get_peripheral_identifier(){
			return peripheral_identifier;
		}
		
		int get_addr_type(){
			return addr_type;
		}
		
		int get_dbm_int(){
			return dbm_int;
		}
		
		int get_tx_power_int(){
			return tx_power_int;
		}
		
		int get_peripheral_idx(){
			return peripheral_idx;
		}

    private:
        boost::thread mThread;
        bool mThreadShouldRun;
		SimpleBLE::Peripheral peripheral;
		std::string peripheral_identifier;
		std::string peripheral_address;
		std::string write_str;
		int addr_type;
		int dbm_int;
		int tx_power_int;
		int peripheral_idx;
		int time_to_scan;
				
		// Store all service and characteristic uuids in a vector.
		std::vector<std::pair<SimpleBLE::BluetoothUUID, SimpleBLE::BluetoothUUID>> uuids;
		int uuid_write_idx;
		
		std::string peripheral_identifier_to_find;
		std::string peripheral_address_to_find;
		std::string uuid_read_to_find;
		std::string uuid_write_to_find;
};

int main(void) {
	char letter;
	std::string ret="";
	ScanClass *Scan=NULL;	
	Scan=new ScanClass();
	
	std::cout << "Started OK" << std::endl;
	while(1) {
		scanf("  %c", &letter );
		if( letter == 'x' ){
			break;
		}else
		if( letter == 'a' ){
			if( !Scan->isScanRunning() ){
				Scan->set_peripheral_address_to_find("E4:65:B8:70:87:6E");
				Scan->set_uuid_read_to_find("66E5FFCE-AA96-4DC9-90C3-C62BBCCD29AC");
				Scan->set_uuid_write_to_find("142F29DD-B1F0-4FA8-8E55-5A2D5F3E2471");
				Scan->Start();
			}
		}
		if( letter == 'b' ){  
			if( Scan->peripheral_is_connected() ){
				/*
				std::cout << "Found peripheral_address: " << Scan->get_peripheral_address() << std::endl;
				std::cout << "Found peripheral_identifier: " << Scan->get_peripheral_identifier() << std::endl;
				std::cout << "Found addr_type: " << Scan->get_addr_type() << std::endl;
				std::cout << "Found dbm_int: " << Scan->get_dbm_int() << std::endl;
				std::cout << "Found tx_power_int: " << Scan->get_tx_power_int() << std::endl;
				*/
				Scan->do_write("Test");
			}
		}else
		if( letter == '0' ){
		}else
		if( letter == '1' ){
		}else
		if( letter == '2' ){
		}
	}
	if( Scan){
		Scan->Stop();
		delete Scan;
	}
	Scan=NULL;  
	return 0;
};