import java.io.BufferedReader;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.LineNumberReader;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.PortUnreachableException;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.util.LinkedList;
import java.util.List;

import javax.sound.midi.MidiChannel;
import javax.sound.midi.MidiSystem;
import javax.sound.midi.MidiUnavailableException;
import javax.sound.midi.Synthesizer;

import be.ac.ulg.montefiore.run.jahmm.ForwardBackwardCalculator;
import be.ac.ulg.montefiore.run.jahmm.Hmm;
import be.ac.ulg.montefiore.run.jahmm.ObservationInteger;
import be.ac.ulg.montefiore.run.jahmm.OpdfIntegerFactory;
import be.ac.ulg.montefiore.run.jahmm.io.FileFormatException;
import be.ac.ulg.montefiore.run.jahmm.io.HmmReader;
import be.ac.ulg.montefiore.run.jahmm.io.OpdfIntegerReader;

public class Recognise {
	
	// MotionStar Comms
	private final static byte MSG_RUN_CONTINUOUS = 104;			
	private final static int PORT_NUMBER = 5000;				
	private InetAddress address;								
	private DatagramSocket socket;								
	private	final static String IP_ADDRESS = "10.74.192.9";
	MotionStar m = new MotionStar();
	
	//Observation list from one gesture performance to be compared against a saved HMM
	public List<ObservationInteger> sequencesInstanceX, sequencesInstanceY, sequencesInstanceZ;
	
	// Current received value from sensor to add to list
	public int receivedX = 0; 
	public int receivedY = 0;
	public int receivedZ = 0;
	
	// Variables
	public boolean firstElement = true;  	
	public int noOfGestures, instrumentNumber;
	public FileReader fileReaderX, fileReaderY, fileReaderZ;
	public FileWriter testWriterX, testWriterY, testWriterZ;
	public OpdfIntegerReader opdfReader;

	//HMM
	public OpdfIntegerFactory factory;
	public Hmm<ObservationInteger> hmmX, hmmY, hmmZ;
	public ForwardBackwardCalculator fbc;	
	public Hmm<ObservationInteger> hmm3X, hmm3Y, hmm3Z;
	public Hmm<ObservationInteger> hmm1X, hmm1Y, hmm1Z;
	public Hmm<ObservationInteger> hmm2X, hmm2Y, hmm2Z;
	public Hmm<ObservationInteger> hmm4X, hmm4Y, hmm4Z;
	public Hmm<ObservationInteger> hmm5X, hmm5Y, hmm5Z;

	
	public double probabilityX = 0;
	public double probabilityY = 0;
	public double probabilityZ = 0;
	public double bestMatchProb = 0.00000000000000000000000001;
	public int bestMatchNo = -1;


	public static void main (String args[]) throws MotionStarException, InterruptedException, IOException, FileNotFoundException, FileFormatException, MidiUnavailableException{
		
		Recognise one = new Recognise();
	}
	
	public Recognise() throws FileFormatException, IOException, MotionStarException, InterruptedException, MidiUnavailableException{

		m.connect();
		Rec();
	}
	
	public void Rec() throws IOException, FileFormatException, MotionStarException, InterruptedException, MidiUnavailableException{

		countGestures();
		String[] gestureName = new String[noOfGestures];
		String[] instrumentName = new String[noOfGestures];
		
		FileInputStream in = new FileInputStream("GestureList.txt");
		FileInputStream im = new FileInputStream("MIDI.txt");
		BufferedReader br = new BufferedReader(new InputStreamReader(in));
		BufferedReader bm = new BufferedReader(new InputStreamReader(im));
		
		for(int i = 1; i<gestureName.length;i++ ){		
			gestureName[i] = br.readLine();
			}			
		in.close();
		br.close();
		
		System.out.println("");
		System.out.println("Your saved gestures are:");
		
		for (int i = 1; i<gestureName.length; i++){
			System.out.println("Gesture #[" + i + "] = " + gestureName[i]);
			}
		
		for(int i = 1; i< instrumentName.length;i++ ){		
			instrumentName[i] = bm.readLine();
			}
		
		im.close();
		bm.close();
				 
		for (int i=1; i<gestureName.length;i++){
			readHmm(i);
			}
		
		for (int j = 1; j<11;j++){
			sequencesInstanceX = new LinkedList<ObservationInteger>();
			sequencesInstanceY = new LinkedList<ObservationInteger>();
			sequencesInstanceZ = new LinkedList<ObservationInteger>();
			
			System.out.println("");
			System.out.println("Recognition iteration " +j);
			System.out.println("Press Enter, and perform a saved gesture");
			System.in.read();
			
			runContinuous(sequencesInstanceX, sequencesInstanceY, sequencesInstanceZ);
			
			for (int i = 1; i <gestureName.length;i++){
				evaluate(sequencesInstanceX, sequencesInstanceY, sequencesInstanceZ, i);
				}
		
			if (bestMatchNo <= 0){
				System.out.println("*Unable to recognise gesture!*");
				System.out.println("Try again.");			
				}
			else if (bestMatchNo > 0){
				System.out.println("Best match is: "+ gestureName[bestMatchNo]+ " ("+instrumentName[bestMatchNo]+")");
				
				if(instrumentName[bestMatchNo].equalsIgnoreCase("guitar")){
					instrumentNumber = 27;
					bestMatchNo = -1;
					bestMatchProb = 0.00000000000000000000000001;
				}
				else if (instrumentName[bestMatchNo].equalsIgnoreCase("piano")){
					instrumentNumber = 0;
					bestMatchNo = -1;
					bestMatchProb = 0.00000000000000000000000001;
				}
				else if (instrumentName[bestMatchNo].equalsIgnoreCase("bass")){
					instrumentNumber = 33;
					bestMatchNo = -1;
					bestMatchProb = 0.00000000000000000000000001;
				}
				else if (instrumentName[bestMatchNo].equalsIgnoreCase("synth")){
					instrumentNumber = 81;
					bestMatchNo = -1;
					bestMatchProb = 0.00000000000000000000000001;
				}
				else if (instrumentName[bestMatchNo].equalsIgnoreCase("organ")){
					instrumentNumber = 19;
					bestMatchNo = -1;
					bestMatchProb = 0.00000000000000000000000001;
				}					
				midiOutput(instrumentNumber);
			}
		}
		
		System.out.println("");
		m.disconnect();
		System.out.println("Restart RECOGNISE program to continue recognising gestures!");
		System.exit(0);	
		}
	
	
	//Counts the the number of saved gestures to give a correct length to the array
	public int countGestures() throws IOException{
	
		File file = new File("GestureList.txt");
		LineNumberReader lnr = new LineNumberReader(new FileReader(file));
		lnr.skip(Long.MAX_VALUE);
		noOfGestures = lnr.getLineNumber() + 1;	
		lnr.close();
		
		return noOfGestures;
	}

	
	//Reads each HMM from the saved text files
	public void readHmm(int hmmNo)throws FileFormatException, FileNotFoundException, IOException{
		
		String[] gestureName = new String[noOfGestures];
		FileInputStream in = new FileInputStream("GestureList.txt");
		BufferedReader br = new BufferedReader(new InputStreamReader(in));
		
		for(int i = 1; i<gestureName.length;i++ ){		
			gestureName[i] = br.readLine();
			}	
		in.close();
		br.close();
		
		fileReaderX = new FileReader(gestureName[hmmNo] + " X.txt");
		fileReaderY = new FileReader(gestureName[hmmNo] + " Y.txt");
		fileReaderZ = new FileReader(gestureName[hmmNo] + " Z.txt");
		opdfReader = new OpdfIntegerReader();

		
		switch(hmmNo){
		case 1:
			hmm1X = HmmReader.read(fileReaderX, opdfReader );
			hmm1Y = HmmReader.read(fileReaderY, opdfReader );
			hmm1Z = HmmReader.read(fileReaderZ, opdfReader );
			break;
			
		case 2:
			hmm2X = HmmReader.read(fileReaderX, opdfReader );
			hmm2Y = HmmReader.read(fileReaderY, opdfReader );
			hmm2Z = HmmReader.read(fileReaderZ, opdfReader );
			break;
		
		case 3:
			hmm3X = HmmReader.read(fileReaderX, opdfReader );
			hmm3Y = HmmReader.read(fileReaderY, opdfReader );
			hmm3Z = HmmReader.read(fileReaderZ, opdfReader );
			break;
		
		case 4:
			hmm4X = HmmReader.read(fileReaderX, opdfReader );
			hmm4Y = HmmReader.read(fileReaderY, opdfReader );
			hmm4Z = HmmReader.read(fileReaderZ, opdfReader );
			break;
		
		case 5:
			hmm5X = HmmReader.read(fileReaderX, opdfReader );
			hmm5Y = HmmReader.read(fileReaderY, opdfReader );
			hmm5Z = HmmReader.read(fileReaderZ, opdfReader );
			break;
			
			default:
				System.out.println("Error in readhHmm()");
				break;
		}
			}
		
		
	public void runContinuous(List<ObservationInteger>sequencesX, List<ObservationInteger>sequencesY, List<ObservationInteger>sequencesZ) throws MotionStarException, InterruptedException{
		
		setSocket();
		sendMessage(MSG_RUN_CONTINUOUS);
		System.out.println("Capturing...");
			
		for (int i=0;i<101;i++){
				
			receiveMessage();
				
			if (firstElement == true){					
				firstElement = false;
				}						
			else {				
				
				ObservationInteger x = new ObservationInteger(receivedX);
				sequencesX.add(x);
				ObservationInteger y = new ObservationInteger(receivedY);
				sequencesY.add(y);
				ObservationInteger z = new ObservationInteger(receivedZ);
				sequencesZ.add(z);	

				Thread.sleep(50);		
				}	 	
			}
			
			firstElement = true;
			System.out.println("");
			System.out.println("Stopped capturing");
			System.out.println("");
	
	}
		
	public void evaluate (List<ObservationInteger> x, List<ObservationInteger> y, List<ObservationInteger> z, int hmmNo){
		switch (hmmNo){
		case 1:
			probabilityX = getProbability(x, hmm1X, probabilityX);
			probabilityY = getProbability(y, hmm1Y, probabilityY);
			probabilityZ = getProbability(z, hmm1Z, probabilityZ);
			break;
		
		case 2:
			probabilityX = getProbability(x, hmm2X, probabilityX);
			probabilityY = getProbability(y, hmm2Y, probabilityY);
			probabilityZ = getProbability(z, hmm2Z, probabilityZ);
			break;
		case 3:
			probabilityX = getProbability(x, hmm3X, probabilityX);
			probabilityY = getProbability(y, hmm3Y, probabilityY);
			probabilityZ = getProbability(z, hmm3Z, probabilityZ);	
			break;
		
		case 4:
			probabilityX = getProbability(x, hmm4X, probabilityX);
			probabilityY = getProbability(y, hmm4Y, probabilityY);
			probabilityZ = getProbability(z, hmm4Z, probabilityZ);
			break;
			
		case 5:
			probabilityX = getProbability(x, hmm5X, probabilityX); 
			probabilityY = getProbability(y, hmm5Y, probabilityY);
			probabilityZ = getProbability(z, hmm5Z, probabilityZ);
			break;
		
		default:
				System.out.println("Error in evaluate()");
				break;
		}
	
		//System.out.println("HMM " +hmmNo + " = X: " + probabilityX +" Y: " + probabilityY  +" Z: "+ probabilityZ);
		
		if ((probabilityX + probabilityY + probabilityZ) !=0 &&
				(probabilityX + probabilityY + probabilityZ) > bestMatchProb) 
		{
			bestMatchProb = probabilityX + probabilityY + probabilityZ;
			bestMatchNo = hmmNo;
		}
	}

	
	public double getProbability(List<ObservationInteger> sequence, Hmm<ObservationInteger> hmm, double probability){
		
		fbc = new ForwardBackwardCalculator(sequence, hmm);
		probability = fbc.probability();
		return probability;	
	}
	
	public void midiOutput(int instrumentNumber) throws MidiUnavailableException, InterruptedException{
		
		Synthesizer synth = MidiSystem.getSynthesizer();
		synth.open();
		
		MidiChannel[] channels = synth.getChannels();

		channels[0].programChange(instrumentNumber);
		if (instrumentNumber == 33 | instrumentNumber == 81){
			channels[0].noteOn(45, 120);
			Thread.sleep(1000);
	        channels[0].noteOff(45);
		} 
		else{
			channels[0].noteOn(60, 120);
			Thread.sleep(1000);
			channels[0].noteOff(60);
			}		
	}
	
	private void setSocket() throws MotionStarException	{
		try	{
			socket = new DatagramSocket();
			socket.setSoTimeout(300);
		}
		catch (SocketException e)	{
			throw new MotionStarException("Cannot connect socket on port " + PORT_NUMBER);
		}
	}

public void sendMessage(byte Message) throws MotionStarException {
		byte [] message = new byte [16];
		message [8] = Message;
		message [10] = 3;

		try		{
			address = InetAddress.getByName(IP_ADDRESS);
		}
		catch (UnknownHostException e)		{
			throw new MotionStarException("You do not appear to be connected to a network");
		}

		try		{
			DatagramPacket packet = new DatagramPacket(message, message.length, address, PORT_NUMBER);
			socket.send(packet);
		}
		catch (PortUnreachableException e)		{
			throw new MotionStarException("Port unreachable error with sending");
		}
		catch (IOException e)		{
			throw new MotionStarException("IO error with sending");
		}
	}

public void receiveMessage() throws MotionStarException	{
		byte [] received = new byte [128];

		try		{
			address = InetAddress.getByName(IP_ADDRESS);
		}
		catch (UnknownHostException e)		{
			throw new MotionStarException("You do not appear to be connected to a network");
		}

		try	{
			DatagramPacket packet = new DatagramPacket(received, received.length, address, PORT_NUMBER);
			socket.receive(packet);
			received = packet.getData();
		}
		catch (PortUnreachableException e)	{
			throw new MotionStarException("Port unreachable error with sending");
		}
		catch (IOException e)		{
			throw new MotionStarException("IO error with sending");
		}
		if (firstElement == false){				//Doesn't capture first value of 0
			receivedX = (received[18]+20);		// Add 20 to ensure positivity
			receivedY = (received[20]+20);		// Add 20 to ensure positivity
			receivedZ = (received[22]+20);		// Add 20 to ensure positivity
			}
	}
}