public class MAVUdpCommNIO implements IMAVComm {

    private DataModel model = null;
    private ByteChannel channel = null;
    private MAVLinkToModelParser parser = null;
    private boolean isConnected = false;
    private static MAVUdpCommNIO com = null;

    private AtomicReference<DatagramSocket> socketRef = new AtomicReference<>();
    private int serverPort;
    private int hostPort;
    private InetAddress hostAdd;
    private DatagramPacket sendPacket;
    private DatagramPacket receivePacket;

    public static MAVUdpCommNIO getInstance(DataModel model, String peerAddress, int peerPort, String bindAddress, int bindPort) {
        if (com == null) {
            com = new MAVUdpCommNIO(model, bindPort);
        }
        return com;
    }

    private MAVUdpCommNIO(DataModel model, int port) {
        this.model = model;
        this.parser = new MAVLinkToModelParser(model, this);
        this.serverPort = port;
    }

    public boolean open() {

        if (channel != null && channel.isOpen()) {
            isConnected = true;
            return true;
        }

        try {
            final DatagramSocket socket = new DatagramSocket(null);
            socket.setBroadcast(true);
            socket.setReuseAddress(true);
            socket.bind(new InetSocketAddress(serverPort));
            socketRef.set(socket);
            channel = new ByteChannel() {

            @Override
            public int read(ByteBuffer readData) throws IOException {
                final DatagramSocket socket = socketRef.get();
                if (socket == null)
                    return 0;

                if (receivePacket == null)
                    receivePacket = new DatagramPacket(readData.array(), readData.array().length);
                else
                    receivePacket.setData(readData.array());

                socket.receive(receivePacket);
                hostAdd = receivePacket.getAddress();
                hostPort = receivePacket.getPort();
                return receivePacket.getLength();
            }

            @Override
            public boolean isOpen() {
                return socket.isConnected();
            }

            @Override
            public void close() throws IOException {
                final DatagramSocket socket = socketRef.get();
                if (socket != null) {
                    if (socket.isConnected())
                        socket.disconnect();
                    socket.close();
                }
            }

            @Override
            public int write(ByteBuffer buffer) throws IOException {
                final DatagramSocket socket = socketRef.get();
                if (socket == null)
                    return 0;

                try {
                    if (hostAdd != null) { // We can't send to our sister until they
                        // have connected to us
                        if (sendPacket == null)
                            sendPacket = new DatagramPacket(buffer.array(), buffer.array().length, hostAdd, hostPort);
                        else {
                            sendPacket.setData(buffer.array(), 0, buffer.array().length);
                            sendPacket.setAddress(hostAdd);
                            sendPacket.setPort(hostPort);
                        }
                        socket.send(sendPacket);
                    }
                } catch (Exception e) {
                    e.printStackTrace();
                }
                return 0;
            }

        };

            msg_heartbeat msg = new msg_heartbeat(255, 0);
            msg.isValid = true;
            write(msg);

            msg_system_time time = new msg_system_time(255, 0);
            time.time_unix_usec = System.currentTimeMillis() / 1000L;
            time.isValid = true;
            write(time);

            parser.start(channel);
            isConnected = true;
            System.out.println("UDP connected...");
            return true;
        } catch (Exception e) {
            System.out.println("Cannot connect to Port: " +e.getMessage()+" " + serverPort);
            close();
            isConnected = false;
        }

        return false;
    }

    public List<Message> getMessageList() {
        return parser.getMessageList();
    }

    @Override
    public Map<Class<?>, MAVLinkMessage> getMavLinkMessageMap() {
        if (parser != null)
            return parser.getMavLinkMessageMap();
        return null;
    }

    public void write(MAVLinkMessage msg) throws IOException {
        ByteBuffer buf = ByteBuffer.wrap(msg.encode());
        channel.write(buf);
    }

    @Override
    public void addMAVLinkMsgListener(IMAVLinkMsgListener listener) {
        parser.addMAVLinkMsgListener(listener);
    }

    @Override
    public void addModeChangeListener(IMSPModeChangedListener listener) {
        parser.addModeChangeListener(listener);
    }

    public boolean isConnected() {
        return isConnected;
    }

    public DataModel getModel() {
        return model;
    }

    public void close() {
        if (parser != null)
            parser.stop();
        try {
            if (channel != null) {
                channel.close();
            }
        } catch (IOException e) {

        }
        LockSupport.parkNanos(1000000000);
    }

public static void main(String[] args) {
    MAVUdpCommNIO comm = new MAVUdpCommNIO(new DataModel(), 14550);

    comm.open();

    long time = System.currentTimeMillis();

    try {

        ModelCollectorService colService = new ModelCollectorService(comm.getModel());
        colService.start();

        System.out.println("Started");

        while (System.currentTimeMillis() < (time + 60000)) {

            // comm.model.state.print("NED:");
            // System.out.println("REM="+comm.model.battery.p+" VOLT="+comm.model.battery.b0+" CURRENT="+comm.model.battery.c0);

            if (comm.model.sys.isStatus(Status.MSP_CONNECTED))
                System.out.println("ANGLEX=" + comm.model.attitude.aX + " ANGLEY=" + comm.model.attitude.aY + " " + comm.model.sys.toString());

            Thread.sleep(1000);

        }

        for (Message m : comm.getMessageList()) {
            System.out.println(m.severity + ": " + m.msg);
        }

        colService.stop();
        comm.close();

        ExecutorService.shutdown();

        System.out.println(colService.getModelList().size() + " models collected");

        // for(int i=0;i<colService.getModelList().size();i++) {
        // DataModel m = colService.getModelList().get(i);
        // System.out.println(m.attitude.aX);
        // }
    } catch (Exception e) {
        comm.close();
        // TODO Auto-generated catch block
        e.printStackTrace();
    }
}