package lejos.pc.comm;

import java.io.*;

/**
 * Implementation of NXTComm using the the jbluez library 
 * on Linux or Unix systems. 
 * 
 * Should not be used directly - use NXTCommFactory to create
 * an appropriate NXTComm object for your system and the protocol
 * you are using.
 *
 */
public class NXTCommBluez implements NXTComm {

	private static final String BDADDR_ANY = "00:00:00:00:00:00";

	private int sk = -1;
	private int lenRemaining = 0;
	byte[] savedData = null;

	static {
		System.loadLibrary("jbluez");
	}
	
	public NXTInfo[] search(String name, int protocol) {
		String[] btString = null;
		
		try {
			btString = search(name);
		} catch (BlueZException e) {
			System.err.println(e.getMessage());	
		}
		if (btString == null) return new NXTInfo[0];
		else {
			NXTInfo[] nxts = new NXTInfo[btString.length];
			for(int i=0;i<btString.length;i++) {
				NXTInfo nxtInfo = new NXTInfo();
				if (btString[i] == null) {
					System.err.println("Null btString");
					return new NXTInfo[0];
				}
				int sep = btString[i].indexOf("::");
				//System.out.println("Setting address to " + btAddress);
				nxtInfo.btDeviceAddress =  btString[i].substring(sep+2);
				nxtInfo.name = btString[i].substring(0, sep);
				nxtInfo.protocol = NXTCommFactory.BLUETOOTH;
				nxtInfo.btResourceString = btString[i];
				
				nxts[i] = nxtInfo;			
			}
			return nxts;
		}
	}

	public void close() throws IOException{
		try {
			rcSocketShutdown(sk);
		} catch (IOException ioe) {
			//System.err.println("Shutdown failed");
		}
		if (sk != -1) rcSocketClose(sk);
		sk = -1;
	}

	public boolean open(NXTInfo nxt) {
		lenRemaining = 0;
		savedData = null;
		
		try {
			open(BDADDR_ANY, nxt.btDeviceAddress, 1);
			return true;
		} catch (BlueZException e) {
			System.err.println("Error from open: " + e.getMessage());
			return false;
		}
		
	}

	public byte [] sendRequest(byte[] request, int replyLen) throws IOException {
		
		// add lsb & msb
		byte[] lsb_msb = new byte[2];
		lsb_msb[0] = (byte) request.length;
		lsb_msb[1] = (byte) 0x00;
		request = concat(lsb_msb, request);
	
	    rcSocketSend(sk, request);
		
		if (replyLen == 0) return new byte[0];
		
		byte[] data = null;
	    data = rcSocketRecv(sk);
	
		// remove lsb & msb
		data = subArray(data, 2, data.length);

		return data;
	}

	private void open(String l_bdaddr, String r_bdaddr, int channel) throws BlueZException {
		boolean ok = false;

		try {
			//System.out.println("Creating socket");
			sk = rcSocketCreate();
			//System.out.println("Binding");
			rcSocketBind(sk, l_bdaddr);
			//System.out.println("Connecting");
			rcSocketConnect(sk, r_bdaddr, channel);

			ok = true;
		} finally {
			if (!ok) {
				if (sk != -1) {
					try {
						rcSocketClose(sk);
					} catch (IOException ioe) {}
					sk = -1;
				}
			}
		}
	}
	
	private byte[] concat(byte[] data1, byte[] data2) {
		int l1 = data1.length;
		int l2 = data2.length;
		
		byte[] data = new byte[l1 + l2];
		System.arraycopy(data1, 0, data, 0, l1);
		System.arraycopy(data2, 0, data, l1, l2);
		
		return data;
	}
		
	
	private byte[] subArray(byte[] data, int start, int end) {	

		byte[] result = new byte[end - start];
		System.arraycopy(data, start, result, 0, end - start);

		return result;
	}
	
	public byte [] read () throws IOException {
		byte [] availData = rcSocketRecv(sk); // Can read multiple packets
		int len = (availData == null ? 0 : availData.length);
		int newLenRemaining = 0;
		
		//System.out.println("Length = " + availData.length);
		
		if (len == 0) return null; // EOF
		
		if (len <= lenRemaining) { // Just more of previous packet
			lenRemaining -= len;
			return availData;
		}
		
		// Append data to saved data, if any
		if (savedData != null) {
			availData = concat(savedData, availData);
			len = availData.length;
			savedData = null;
		}
		
		// Make sure we have the header and at least one byte of data
		while (len < 3) {
			byte [] moreData = rcSocketRecv(sk);
			if (moreData == null || moreData.length == 0) return null;
			availData = concat(availData, moreData);
			len = availData.length;		
		}
		
		int i = lenRemaining;
		int dataLen = lenRemaining;
		
		while (i < len) { // Calculate length skipping packet headers
			if (len - i < 3) {
				//Save the remaining data
				savedData = new byte[len - i];
				for(int j=0;j<len-i;j++) savedData[j] = availData[i+j];
				break;
			}
			int lsb = availData[i++];
			int msb = availData[i++];
			int packetLen = ((lsb & 0xFF) + (msb << 8));
			
			//System.out.println("Packet length is " + packetLen);
			
            if (i + packetLen <= len) {
            	dataLen += packetLen;
            } else {
            	dataLen += (len-i);
            	newLenRemaining = packetLen - (len - i);
            }
            i += packetLen;
		}
		
		//System.out.println("data length is " + dataLen);
		
		byte [] data = new byte [dataLen];
		
		// Copy any data from previous packet
		for(i=0;i<lenRemaining;i++) data[i] = availData[i];

		int j = i;
		
		// Copy any available packets. The last one may be incomplete.		
		while (i < len-2) {
			int lsb = availData[i++];
			int msb = availData[i++];
			int packetLen = ((lsb & 0xFF) + (msb << 8));
            for(int k = 0;k<packetLen && i+k < len;k++) data[j++] = availData[i+k];
            i += packetLen;
		}
		
		lenRemaining = newLenRemaining;
		
		//System.out.println("Length remaining is " + lenRemaining);
		return data;
	}
	
	public int available() throws IOException {
		return 0;
	}
	
	public void write(byte[] data) throws IOException {
		byte[] lsb_msb = new byte[2];
		lsb_msb[0] = (byte) data.length;
		lsb_msb[1] = (byte) ((data.length >> 8) & 0xff);
		rcSocketSend(sk, concat(lsb_msb, data));
	}
	
	public OutputStream getOutputStream() {
		return new NXTCommOutputStream(this);		
	}
	
	public InputStream getInputStream() {
		return new NXTCommInputStream(this);		
	}
	
	native private String[] search(String name) throws BlueZException;
	
	native private int rcSocketCreate() throws BlueZException;

	native private void rcSocketBind(int sk, String bdaddr) throws BlueZException;

	native private void rcSocketConnect(int sk, String bdaddr, int channel) throws BlueZException;

	native public void rcSocketSend(int sk, byte[] data) throws IOException;

	native public byte[] rcSocketRecv(int sk) throws IOException;

	native private void rcSocketShutdown(int sk) throws IOException;

	native private void rcSocketClose(int sk) throws IOException;

}
