#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 "serial_io.h"
#include "Socket.h"

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";
}

class Config
{
public:
    std::string portFileName = "/dev/ttyUSB0";
    unsigned short port = 6856;
    int baud = 0000017;
};

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+1) config->portFileName = argv[i+1];
            else return -1;
        }
        else if (std::string(argv[i]) == "--port" || std::string(argv[i]) == "-P")
        {
            if(argc > i+1) config->portFileName = atoi(argv[i+1]);
            else return -1;
        }
         else if (std::string(argv[i]) == "--baud" || std::string(argv[i]) == "-b")
        {
            if(argc > i+1) config->baud = atoi(argv[i+1]);
            else return -1;
        }
    }
    return 0;
}

void acceptThreadFunction( TCPServerSocket* servSock,  std::vector<TCPSocket*>* clientSockets, volatile bool* stop )
{
    while(!(*stop)) 
    {   // Run forever
        clientSockets->push_back(servSock->accept());       // Wait for a client to connect
        clientSockets->back()->send("UVOS serial port multiplexer v0.1\n", sizeof("uvos serial port multiplexer v0.1\n")-1);
        std::cout<<"got client\n";
    }
}

int main(int argc, char* argv[])
{
    Config config;
    
    if(parseCmdArgs(argc, argv, &config) != 0) return -1;
    
    std::cout<<"UVOS serial mulitplexer v0.1\n";
    
    int serial = serialport_init(config.portFileName.c_str(), config.baud);

    std::vector<TCPSocket*> clientSockets;
    
    std::cout<<"opening TCP socet on port "<<config.port<<'\n';
    TCPServerSocket servSock(config.port, 5, true);     // Server Socket object
    
    volatile bool threadStopper = false;
    std::thread acceptThread(acceptThreadFunction, &servSock, &clientSockets,  &threadStopper);
    
    char buffer[256];
    
    std::cout<<"starting loop\n";
    
    while(!stop)
    {
        int readlen = sRead(serial, buffer, 256);
        //std::cout<<clientSockets.size()<<std::endl;
        for(unsigned int i = 0; i < clientSockets.size(); i++)
        {
            try
            {

                if(readlen > 0) 
                {
                    std::cout<<"sending: ";
                    for( int j = 0; j < readlen; j++  )std::cout<<buffer[j];
                    clientSockets[i]->send(buffer, readlen);
                    std::cout<<std::endl;
                }
                
                char inBuffer[256];
                int reclen = clientSockets[i]->recv(inBuffer, 256);
                if( reclen > 0 ) 
                {
                    std::cout<<"rec "<<reclen<<'\n';
                    for( int j = 0; j < reclen; j++  )std::cout<<inBuffer[j];
                    sWrite(serial, inBuffer, reclen);
                }
                else if(reclen == 0)
                {
                    std::cout<<"disconnect client "<<i<<'\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(20));
    }
    
    threadStopper = true;
    acceptThread.join();
    for(unsigned int i = 0; i < clientSockets.size(); i++) clientSockets[i]->cleanUp();
    servSock.cleanUp();
    return 0;
}