#include <iostream>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <string>
#include <cstdlib>
#include <vector>
#include <thread>
#include <signal.h>

#include "serial_io.h"
#include "Socket.h"

#define VERSION "v0.4"

bool stop = false;

void intHandler(int dummy) 
{
    stop = true;
}

static void printUsage()
{
    std::cout<<"usage mulitplexer [option]\n\
 Available options:\n\
 -h, --help       print this help\n\
 -p, --serialport serial port device to use\n\
 -P, --port       tcp port to use\n\
 -b, --baud       set baud rate with termios id\n\
 -r, --rates      list Available baud rates\n\
 -s, --sinkless   run without serial port\n";
}

struct Config
{
    std::string portFileName = "/dev/ttyUSB0";
    unsigned short port = 6856;
    int baud = B9600;
    bool noSerial = false;
	bool verbose = false;
};

static int parseCmdArgs(int argc, char** argv, Config *config)
{
    for (int i = 1; i < argc; i++)
    {
        if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h")
        {
            printUsage();
            return -1;
        }
        else if (std::string(argv[i]) == "--serialport" || std::string(argv[i]) == "-p")
        {
            if(argc > i) config->portFileName = argv[i+1];
            else return -1;
        }
        else if (std::string(argv[i]) == "--port" || std::string(argv[i]) == "-P")
        {
            if(argc > i) config->port = atoi(argv[i+1]);
            else return -1;
        }
        else if (std::string(argv[i]) == "--baud" || std::string(argv[i]) == "-b")
        {
            if(argc > i) config->baud = atoi(argv[i+1]);
            else return -1;
        }
        else if (std::string(argv[i]) == "--sinkless" || std::string(argv[i]) == "-s" )
        {
            config->noSerial=true;
        }
        
        else if (std::string(argv[i]) == "-r" || std::string(argv[i]) == "--rates")
        {
            printRates();
            return -1;
        }
        else if (std::string(argv[i]) == "-v" || std::string(argv[i]) == "--verbose")
        {
            config->verbose=true;
        }
    }
    return 0;
}

void acceptThreadFunction( TCPServerSocket* servSock,  std::vector<TCPSocket*>* clientSockets )
{
    while(!stop) 
    {   
        TCPSocket* newSock = servSock->accept();
        if(newSock != nullptr)
        {
            clientSockets->push_back(newSock);       // Wait for a client to connect
            clientSockets->back()->send("UVOS serial multiplexer " VERSION "\n", sizeof("UVOS serial multiplexer " VERSION "\n")-1);
            std::cout<<"got client\n";
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
}

int main(int argc, char* argv[])
{
    Config config;
    
    if(parseCmdArgs(argc, argv, &config) != 0) return -1;
    
    std::cout<<"UVOS serial mulitplexer "<<VERSION<<'\n';
    
    int serial = -1;
    if(!config.noSerial)
    {
        std::cout<<"Using serial port: "<<config.portFileName<<" at "<<config.baud<<" baud\n";
        serial = serialport_init(config.portFileName.c_str(), config.baud);
        if(serial == -1) return 1;
		tcflush(serial, TCIOFLUSH);
    }
    else std::cout<<"Sinkless mode\n";

    std::vector<TCPSocket*> clientSockets;
    
    std::thread* acceptThread;
    TCPServerSocket* servSock;
    
    std::cout<<"opening TCP socket on port "<<config.port<<'\n';
    try
    {
        servSock = new TCPServerSocket(config.port, 5, true);     // Server Socket object
        servSock->setBlocking(false);
        acceptThread = new std::thread(acceptThreadFunction, servSock, &clientSockets);
    }
    catch(SocketException &e)
    {
        std::cerr<<"Could not open port"<<std::endl;
        return 1;
    }
    
    char buffer[4096];
    
    signal(SIGINT, intHandler);
    signal(SIGTERM, intHandler);
    
    std::cout<<"starting loop\n";
    
    while(!stop)
    {
        int readlen = sRead(serial, buffer, 4096);
        for(unsigned int i = 0; i < clientSockets.size(); i++)
        {
            try
            {

                if(readlen > 0) 
                {
					if(config.verbose) 
					{
						std::cout<<"bcst: ";
						for( int j = 0; j < readlen; j++  )std::cout<<buffer[j];
						std::cout<<std::endl;
					}
                    clientSockets[i]->send(buffer, readlen);
                }
                
                char inBuffer[4096];
                int reclen = clientSockets[i]->recv(inBuffer, 4096);
                if( reclen > 0 ) 
                {
                    if(config.verbose)
					{
						std::cout<<"rec: ";
						for( int j = 0; j < reclen; j++  )std::cout<<inBuffer[j];
					}
                    
                    if(strncmp( inBuffer, "bcst: ", 6 ) == 0)
                    {
                        if(config.verbose) 
						{
							std::cout<<"bcst: ";
							for( int j = 6; j < reclen; j++  )std::cout<<inBuffer[j];
							std::cout<<std::endl;
						}
                        for(unsigned int j = 0; j < clientSockets.size(); j++) if(i != j) clientSockets[j]->send(inBuffer+6, reclen-6);
                        
                    }
                    else
                    {
						if(config.verbose) 
						{
							std::cout<<"wrote \"";
							for( int j = 0; j < reclen; j++  )std::cout<<inBuffer[j];
							std::cout<<"\" to serial \n";
						}
                        sWrite(serial, inBuffer, reclen);
                    }
                    
                    if(config.verbose) std::cout<<std::endl;
                }
                else if(reclen == 0)
                {
                    std::cout<<"client "<<i<<" disconnected"<<'\n';
                    clientSockets[i]->cleanUp();
                    clientSockets.erase(clientSockets.begin()+i);
                    i--;
                    if(i < 0) i=0;
                }
            }
            catch(SocketException &e)
            {
                std::cout<<e.what()<<std::endl;
                clientSockets[i]->cleanUp();
                clientSockets.erase(clientSockets.begin()+i);
                i--;
                if(i < 0) i=0;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    
    acceptThread->join();
    delete acceptThread;
    for(unsigned int i = 0; i < clientSockets.size(); i++) clientSockets[i]->cleanUp();
    servSock->cleanUp();
    delete servSock;
    return 0;
}