int Socket::connect(int timeoutInMillis) {
            assert(serverInfo != NULL && "Socket is already connected");
            setBlocking(false);
            ::connect(socketId, serverInfo->ai_addr, serverInfo->ai_addrlen);

            struct timeval tv;
            tv.tv_sec = timeoutInMillis / 1000;
            tv.tv_usec = (timeoutInMillis - tv.tv_sec * 1000) * 1000;
            fd_set mySet, err;
            FD_ZERO(&mySet);
            FD_ZERO(&err);
            FD_SET(socketId, &mySet);
            FD_SET(socketId, &err);
            errno = 0;
            if (select(socketId + 1, NULL, &mySet, &err, &tv) > 0) {
                setBlocking(true);
                return 0;
            }
            setBlocking(true);

            #if  defined(WIN32) || defined(_WIN32) || defined(WIN64) || defined(_WIN64)
            int error =   WSAGetLastError();
            #else
            int error = errno;
            #endif
            return error;

        }

void SocketImpl::connect(const SocketAddress& address, const Poco::Timespan& timeout)
{
	if (_sockfd == POCO_INVALID_SOCKET)
	{
		init(address.af());
	}
	setBlocking(false);
	try
	{
#if defined(POCO_VXWORKS)
		int rc = ::connect(_sockfd, (sockaddr*) address.addr(), address.length());
#else
		int rc = ::connect(_sockfd, address.addr(), address.length());
#endif
		if (rc != 0)
		{
			int err = lastError();
			if (err != POCO_EINPROGRESS && err != POCO_EWOULDBLOCK)
				error(err, address.toString());
			if (!poll(timeout, SELECT_READ | SELECT_WRITE | SELECT_ERROR))
				throw Poco::TimeoutException("connect timed out", address.toString());
			err = socketError();
			if (err != 0) error(err);
		}
	}
	catch (Poco::Exception&)
	{
		setBlocking(true);
		throw;
	}
	setBlocking(true);
}

int EasySocket::connect()
{
    if (server == NULL || m_socket <=0 ) {
        return -1;
        //fprintf(stderr,"ERROR, no such host\n");
    }
    int res = 0;
    //TODO: more intelligence
#ifndef _WIN32
    setBlocking(m_socket,false);

    int counter = 0;
    int sleepMs = 20;
    int waitSec = 1;
    int waitMs = waitSec*1000/sleepMs;
    
    while(counter++ < waitMs)
    {
        res = ::connect(m_socket,(struct sockaddr *) &serv_addr,sizeof(serv_addr));

        checkResult(res);

        if (res == 0)
            break;

        if (m_state == CONNECTION_STATE_IN_PROGRESS)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(sleepMs));
            continue;
        }


        if(m_state != CONNECTION_STATE_IN_PROGRESS && m_state != CONNECTION_STATE_SUCCESS )
            break;
    }

    setBlocking(m_socket,true);
#else
    res = ::connect(m_socket, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
    checkResult(res);
#endif
    if(res == 0){

        struct timeval tv;

        tv.tv_sec = 1;
        tv.tv_usec = 0;

        setsockopt(m_socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv,sizeof(struct timeval));

        m_replySocket = m_socket;
    }
    return res;
}

bool CPPTcpClientSocket::connect(short port, unsigned int addr, int timeout, bool spin){
    std::lock_guard<std::mutex> lockR(recvLock), lockS(sendLock);
    m_port = port;
    m_addr = addr;
    struct sockaddr_in remote;
    memset(&remote, 0, sizeof(remote));
    remote.sin_family = AF_INET;
    remote.sin_addr.s_addr = m_addr;
    remote.sin_port = htons(m_port);
    double tremaining = timeout;
    auto start = std::chrono::system_clock::now();
    bool success = false;    
    do{
        if (!open()){
            break;
        }
        setBlocking(false, false);
        if (::connect(m_sock, (struct sockaddr *)&remote, sizeof(remote)) == 0){
            success = true;
            break;
        }
        if (errno != EINPROGRESS){
            break;
        }
        struct pollfd pfd = {m_sock, POLLWRNORM, 0};
        if (poll(&pfd, 1, (int)tremaining) > 0){
            int optionValue = -1;
            socklen_t optionLength = sizeof(optionValue);
            if (getSocketOption(SOL_SOCKET, SO_ERROR, (void *)&optionValue, &optionLength, false) && optionValue == 0){
                success = true;
                break;
            }
        }
        std::chrono::duration<double, std::milli> dur = std::chrono::system_clock::now() - start;
        tremaining = timeout - dur.count();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        ::shutdown(m_sock, SHUT_RDWR);
        ::close(m_sock);
        m_sock = -1;
        if (!spin) break;
        
    }while((tremaining > 0.0 || timeout < 0) && !sigClose);
    setBlocking(true, false);
    if (success) return true;
    
    if (m_sock != -1){
        ::shutdown(m_sock, SHUT_RDWR);
        ::close(m_sock);
        m_sock=-1;
    }
    return false;
}

/// Will not buffer anything but always send right away. Blocks.
/// Any data that could not be send will block until it can be send or the connection is severed.
void Socket::Connection::SendNow(const char * data, size_t len) {
  bool bing = isBlocking();
  if (!bing) {
    setBlocking(true);
  }
  unsigned int i = iwrite(data, std::min((long unsigned int)len, SOCKETSIZE));
  while (i < len && connected()) {
    i += iwrite(data + i, std::min((long unsigned int)(len - i), SOCKETSIZE));
  }
  if (!bing) {
    setBlocking(false);
  }
}

/// Updates the downbuffer and upbuffer internal variables until upbuffer is empty.
/// Returns true if new data was received, false otherwise.
bool Socket::Connection::flush(){
  bool bing = isBlocking();
  if (!bing){setBlocking(true);}
  while (upbuffer.size() > 0 && connected()){
    iwrite(upbuffer.get());
  }
  if (!bing){setBlocking(false);}
  /// \todo Provide better mechanism to prevent overbuffering.
  if (downbuffer.size() > 1000){
    return true;
  }else{
    return iread(downbuffer);
  }
}

// --------------------------------------------------
void WSAClientSocket::bind(Host &h)
{
	struct sockaddr_in localAddr;

#pragma warning(disable : 4244)
	if ((sockNum = socket (PF_INET, SOCK_STREAM, IPPROTO_TCP)) == -1)
#pragma warning(default : 4244)
		throw SockException("Can`t open socket");

	setBlocking(false);
	setReuse(true);

	memset(&localAddr,0,sizeof(localAddr));
	localAddr.sin_family = AF_INET;
	localAddr.sin_port = htons(h.port);
	localAddr.sin_addr.s_addr = INADDR_ANY;

	if( ::bind (sockNum, (sockaddr *)&localAddr, sizeof(localAddr)) == -1)
		throw SockException("Can`t bind socket");

	if (::listen(sockNum,SOMAXCONN))
		throw SockException("Can`t listen",WSAGetLastError());

	host = h;
}

void HttpMultipartClient::response()
{
    condition.lock();
    responding = true;
    string boundary = "--RhobanMultipartBoundary\r\n";
    ostringstream oss;
    oss << "HTTP/1.0 200 OK\r\n";
    oss << "Content-type: multipart/x-mixed-replace; boundary=" << boundary;
    oss << "\r\n";
    transmitString(oss.str());

    char dummy;
    setBlocking(false);
    while (!isDead()) {
        receive(&dummy, 1);

        condition.wait();
        oss.str("");
        oss << boundary;
        oss << "Content-type: " << type << "\r\n";
        oss << "Content-length: " << frame->length() << "\r\n";
        oss << "\r\n" << *frame;
        transmitString(oss.str());
    }
    condition.unlock();
}

int DLS::setFD (int fd)
{
    fd_ = fd;
    setInterfaceAttribs (B115200, 0);   // set speed to 115,200 bps, 8n1 (no parity)
    setBlocking (0);                    // set no blocking
    return EXIT_SUCCESS;
}

void NetworkSocket::setupBehavior()
{
    setBlocking(false);
    setConnectionReset(false);
    //setBroadCasting(true);
    setReUseAddress(true);
}

/*
 * Creates a socket suitable for raw socket operations.  The socket is
 * bound to the interface specified by the supplied name.  The socket
 * value is placed into the supplied FileDescriptor instance.
 *
 * TODO(chesnutt): consider breaking this into pieces: create a
 * variety of constructors for different socket types, then a generic
 * setBlocking() method followed by polymorphic bind().
 */
static void RawSocket_create(JNIEnv* env, jclass, jobject fileDescriptor,
    jshort protocolType, jstring interfaceName)
{

  ScopedUtfChars ifname(env, interfaceName);
  if (ifname.c_str() == NULL) {
    return;
  }

  memset(&su, 0, sizeof(su));
  su.sll.sll_family = PF_PACKET;
  su.sll.sll_protocol = htons(protocolType);
  su.sll.sll_ifindex = if_nametoindex(ifname.c_str());
  int sock = socket(PF_PACKET, SOCK_DGRAM, htons(protocolType));

  if (sock == -1) {
    ALOGE("Can't create socket %s", strerror(errno));
    jniThrowSocketException(env, errno);
    return;
  }

  jniSetFileDescriptorOfFD(env, fileDescriptor, sock);
  if (!setBlocking(sock, false)) {
    ALOGE("Can't set non-blocking mode on socket %s", strerror(errno));
    jniThrowSocketException(env, errno);
    return;
  }

  int err = bind(sock, &su.sa, sizeof(su));
  if (err != 0) {
    ALOGE("Socket bind error %s", strerror(errno));
    jniThrowSocketException(env, errno);
    return;
  }
}

// --------------------------------------------------
void UClientSocket::open(Host &rh)
{


	sockNum = socket (AF_INET, SOCK_STREAM, IPPROTO_TCP);

	if (sockNum == INVALID_SOCKET)
		throw SockException("Can`t open socket");

	setBlocking(false);

#ifdef DISABLE_NAGLE
	setNagle(false);
#endif
	setBufSize(65535);

	host = rh;

	memset(&remoteAddr,0,sizeof(remoteAddr));

	remoteAddr.sin_family = AF_INET;
	remoteAddr.sin_port = htons(host.port);
	remoteAddr.sin_addr.s_addr = htonl(host.ip);

}

extern "C" JNIEXPORT void JNICALL
Java_java_nio_channels_SocketChannel_configureBlocking(JNIEnv *e,
                                                       jclass,
                                                       jint socket,
                                                       jboolean blocking)
{
  setBlocking(e, socket, blocking);
}

// ######################################################################
int KeyBoard::getKeyAsChar(const bool block)
{
  // make sure we are in the correct blocking/non-blocking mode:
  if (blocking != block) setBlocking(block);
  
  // get the key & return:
  int ch = getc(stdin);
  return ch;
}

// ######################################################################
KeyBoard:: ~KeyBoard()//Destructor
{
  // restore blocking mode:
  setBlocking(true);

  // restore previous keyboard attributes:
  if (tcsetattr(0, TCSANOW, &stored_settings) == -1)
    PLERROR("Cannot tcsetattr");
}

// @brief Send post request
// @param string uri the request uri
// @return If no error occurs returns the number of receive data in bytes,
// Otherwise -1 is returns
int HTTPClient::post( string uri, Chars &body )
{
	string str;
	m_method = "POST";
	str += "POST " + uri + " " + m_version;
	str += "\r\n";
	str += "Host: " + m_host;
	str += "\r\n";
	map<string, string>::iterator iter = m_requestHeader.begin();
	while (iter != m_requestHeader.end()) {
		str += iter->first + ": ";
		str += iter->second;
		str += "\r\n";
		++iter;
	}
	// Set 'Content-length'
	str += "Content-Length: ";
	char buf[25];
	snprintf(buf, 25, "%ld", body.length());
	str += buf;
	str += "\r\n";

	// Headers end
	str += "\r\n";

	Chars data;
	data.assign(str.c_str(), str.length());
	data.append(body.c_str(), body.length());

	if (!isBlocking()) {
		setBlocking();
	}

	// Set receive and send timeout to 5 seconds
	int nTimeout = 5000;
	if (m_host == "127.0.0.1") {
		nTimeout = 100;
	}
	setRcvTimeout(nTimeout);
	setSndTimeout(nTimeout);

	int n = this->send(data.c_str(), data.length());
	if (n == -1) {
		return n;
	}
	//this->shutdown(SHUT_WD);

	/**
	 * Receive response data
	 */

	// Receive all response data to socket buffer including HTTP headers
	while ( this->recv(1024) > 0 )
		;
	return decodeHttpData();
}

// ######################################################################
KeyBoardKey KeyBoard::getKey(const bool block)
{
  // make sure we are in the correct blocking/non-blocking mode:
  if (blocking != block) setBlocking(block);
  
  // get the key & return:
  int ch = getc(stdin);
  if (ch == EOF) return KBD_NONE;
  return fromChar(getc(stdin));
}

void Connection::init( int fd )
{
    if ( d->state != Invalid || fd < 0 )
        return;

    d->fd = fd;
    d->state = Inactive;
    d->timeout = 0;
    d->r = new Buffer;
    d->w = new Buffer;
    setBlocking( false );
}

void SocketServer::catchEvents()
{

    if (selector.wait(sf::seconds(10)))
    {
        if (selector.isReady(listener))//get new client to the selector. maybe welcome msg. put new client into queue. wait second one. then start game.
        {
            auto client = std::make_shared<sf::TcpSocket>();
            client->setBlocking(false);
            if (listener.accept((*client)) == sf::Socket::Done)
            {
                std::cout<<"connected\n";
            }
            sf::Packet packet;
            //needed variables from package
            if(client->receive(packet) == sf::Socket::Done)
            {
                char * msg ="xonnexted!\n";
                client->send(msg, sizeof(msg));
            }//check if you get full packet or not in case of non-blocking
            char * msg ="xonnexted!\n";
            client->send(msg, sizeof(msg));
            clients.push_back(client);
            selector.add(*clients.back());
        }
            for (auto & client : clients)
            {
                if (selector.isReady(*client))
                {
                    std::cout<<"all ok!\n";
                    sf::Packet packet, sendPacket;
                    if (client->receive(packet) == sf::Socket::Done)
                    {
                        //vars from packet
                        //packet >> vars;
                        //some logic
                        char * msg = "okay!\n";
                        //sendPacket<<msg;
                        for (auto & theClient : clients)
                        {
                            theClient->send(msg, sizeof(msg));
                        }
                    }
                    char * msg = "okay!\n";
                    for (auto & theClient : clients)
                    {
                        theClient->send(msg, sizeof(msg));
                    }
                }

        }
    }
}

bool mavlink_usb_importer::initUSB(){

    for(const auto& path : paths)
    {
        usb_fd = open(path.c_str(), O_RDWR | O_NDELAY);
        if (usb_fd < 0) {
            logger.perror("init") << "Open USB device at " << path.c_str() << " (errno: " << errno << ")";
            continue;
        } else {
            break;
        }
    }

    if( usb_fd <= 0 )
    {
        // No valid device found
        return false;
    }
    
    // Set configuration
    setUSBConfig(usb_fd);
    
    // Set blocking I/O
    setBlocking( usb_fd, true );
    
    
    // http://arduino.cc/en/Main/ArduinoBoardNano
    //
    // When the Nano is connected to either a computer running
    // Mac OS X or Linux, it resets each time a connection is made
    // to it from software (via USB). For the following half-second
    // or so, the bootloader is running on the Nano.
    //
    // [...] make sure that the software with which it communicates
    // waits a second after opening the connection and before sending
    // this data.
     
    sleep( config().get<int>("init_sleep", 0) );

    tcflush(usb_fd,TCIOFLUSH);
    
    logger.info("init") << "Initialized USB device connection";

    // Start receiver thread
    if(receiverThread.joinable()) {
        receiverThread.join();
    }
    shouldStopReceiver = false;
    receiverThread = std::thread(&mavlink_usb_importer::receiver, this);

    return true;
}