package fr.esiee.gyroway;


import lejos.nxt.Button;
import lejos.nxt.LCD;
import java.io.*;

import lejos.realtime.RealtimeThread;
import lejos.realtime.ReleaseParameters;
import lejos.realtime.SchedulingParameters;
import lejos.util.Datalogger;

/**
 * Class to handle Bluetooth communication with the legway. Includes a Thread to check incoming data.
 * Includes a method to use the NXTPilot program (not made by us).
 * 
 * @author Alexandre Barsacq
 *
 */
public class BluetoothCommunicator extends RealtimeThread{
	
	private DataInputStream inputStream;
	private DataOutputStream outputStream;

	//Constants for NXTPilot
	private final int NXTPilotFORWARD=1;
	private final int NXTPilotBACKWARD=2;
	private final int NXTPilotLEFT=3;
	private final int NXTPilotRIGHT=4;
	private final int NXTPilotSTOP=5;

	private int reception=NXTPilotSTOP;
	private int old=NXTPilotSTOP;
	private float turnCommand=0;
	private float forwardCommand=0;

	private boolean flag=false;
//	public static Datalogger dl=new Datalogger();
	
	/**
	 * Creates a new instance of BluetoothCommunicator.
	 * @param sched scheduling parameters for real-time thread
	 * @param release release parameters for real-time thread
	 * @param input input bluetooth stream (has to be opened before)
	 * @param output output bluetooth stream (has to be opened before)
	 */
	public BluetoothCommunicator(SchedulingParameters sched, ReleaseParameters release,DataInputStream input,DataOutputStream output)
	{	
		super(sched,release);
		inputStream=input;
		outputStream=output;
	}

	//Running : reads BT stream and updates the values to apply
	public void run(){
		
		while(!Button.ENTER.isPressed()){
			
			readCommandNXTPilot();
			waitForNextPeriod();
			
		}
	}



	/**
	 * Reads commands sent by bluetooth and updates the values to apply.
	 * method designed to use "NXT Pilot" program on Android. Retrieves data
	 * over bluetooth and translates it into legway-usable data. 
	 * 
	 * @throws IOException 
	 */
	private void readCommandNXTPilot()
	{

		try {
			//if nothing available, do nothing
			if(inputStream.available()>0)
			{	
				reception=inputStream.read();	
//				dl.writeLog(reception); 
				
				/*All this part is because of the weird protocol sent by
				 * the App. It translates it into usable data for our robot
				 * 
				 * The protocol sends :
				 * 0 before each command, so we ignore them
				 * 1,2,3,4,5 for : forward, backward,left,right,stop
				 * BUT ALSO, it sends a 2 (backward) each time a new command is
				 * pressed. 
				 * This part of the code makes sure that we don't go backward before
				 * each new command. Sadly, it creates a latency because we have
				 * to apply the old command waiting for the new one.  The latency equals 2 periods 
				 * of the thread (=20 ms as for the time this comment is written)
				 */
				if(reception==NXTPilotBACKWARD && flag!=true)
				{
					reception=old;
					flag=true;
				}
				else if(reception!=0 && flag==true) 
				{
					flag=false;
				}
				old=reception;
				
				//The actual updating of the commands 
				switch(reception)
				{
				case NXTPilotFORWARD :
					turnCommand=0;
					forwardCommand=100;
					break;
				case NXTPilotBACKWARD:
					turnCommand=0;
					forwardCommand=-100;
					break;
				case NXTPilotLEFT:
					turnCommand=100;
					forwardCommand=0;
					break;
				case NXTPilotRIGHT:
					turnCommand=-100;
					forwardCommand=0;
					break;
				case NXTPilotSTOP:
					turnCommand=0;
					forwardCommand=0;
					break;

				default :
					break;

				}
			}
		} catch (IOException e) {
			LCD.drawString("FUCK", 0, 4);
		}


	}

	/**
	 * Returns the last read turning command over bluetooth
	 * @return turning command (between -100 and +100)
	 */
	public float getTurnCommand(){
		return turnCommand;
	}

	/**
	 * Returns the last read forward command over bluetooth
	 * @return forward command (between -100 and +100)
	 */
	public float getForwardCommand(){
		return forwardCommand;

	}


}