/*
 * Created on Jun 19, 2004
 *
 * Written by: Matt Krugman
 * CSM Standard Hybrid Robot Control Architecture
 * Master's Thesis
 */

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Container;
import java.awt.event.*;
import java.io.*;
import javax.comm.*;
import javax.swing.*;

import acroname.*;

/**
 * @author mkrugman
 *
 * HTML documentation coming soon.
 */
public class BrainstemDefault {
	jStem stem;
	static jGP gp;
	PrintStream out;
	public MyKeyboard keyInput = null;
	public ThreadSensorPolling sensorPollThread;
	public String portname;
	public boolean verbose = false;
	public boolean moveFlipDirection = false;
	public int numberOfSensors = 10;
	public double[] sensorReadings;
	public static double[] resultant;
	public static double[] advice;
	public static double[] attractive;
	public static double[] repulsive;

	public static int ANALOG0 = 0;
	public static int ANALOG1 = 1;
	public static int ANALOG2 = 2;
	public static int ANALOG3 = 3;
	public static int ANALOG4 = 4;
	public static int LEFT_SONAR = 5;
	public static int RIGHT_SONAR = 6;

	static final int BAUDRATE = 115200;
	
	static JFrame userInput = new JFrame("Robot Control Window");

	public BrainstemDefault(String portname) {
		PrintStream out = null;
		InputStream inputStream;
		OutputStream outputStream;
		/* store this for use later */
		this.out = out;
		/* build a stem to provide packet protocol */
		stem = new jStem();
		resultant = new double[2];
		try {
			/* try to get the port */
			CommPortIdentifier portId =
				CommPortIdentifier.getPortIdentifier(portname);
			/* open the port */

			SerialPort serialPort =
				(SerialPort) portId.open("CSM Hybrid", 2000);

			serialPort.setSerialPortParams(
				BAUDRATE,
				javax.comm.SerialPort.DATABITS_8,
				javax.comm.SerialPort.STOPBITS_1,
				javax.comm.SerialPort.FLOWCONTROL_NONE);

			/* get the input and output streams */
			inputStream = serialPort.getInputStream();
			outputStream = serialPort.getOutputStream();

			/* set up the stem as a listener on the port */
			serialPort.addEventListener(stem);
			serialPort.notifyOnDataAvailable(true);

			/* tell the stream where to get I/O */
			stem.SetStream(inputStream, outputStream);

			/* build a new GP module interface */
			gp = new jGP(stem, (byte) 2);

			sensorReadings = new double[numberOfSensors];
			sensorPollThread = new ThreadSensorPolling();		
			
			JFrame emergencyStop = new JFrame("The Window");
			emergencyStop.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
			emergencyStop.setSize(300,300);
			JButton killAll = new JButton("Robot Shutdown.");
			killAll.setBackground(Color.RED);
			
			keyInput = new MyKeyboard();
			JFrame typeWindow = new JFrame("Type in me");
			JPanel key = new JPanel();
			JButton drivebutton = new JButton("Robot Drive");
			drivebutton.setBackground(Color.BLUE);
			key.add(drivebutton);
			typeWindow.addKeyListener(keyInput);
			
			killAll.addActionListener(new myButton());

			emergencyStop.getContentPane().add(killAll);
			//emergencyStop.addKeyListener(keyInput);
			emergencyStop.show();
			typeWindow.show();
			
//			JCheckBox jck1 = new JCheckBox("Down shift the robot");
//			Container p = userInput.getContentPane();
//			p.add(drivebutton);
//			p.add(killAll);
//			userInput.pack();

			/* build a new robot hardware setup */
		} catch (Exception e) {
			out.println("GP Error: " + e.toString());
		}
	} /* BrainstemGP constructor */
	
	public static void setupRobotWindow(){
		userInput.setSize(800,800);
		userInput.setVisible(true);
		userInput.getContentPane().setLayout(new BorderLayout(10,7));
		WindowListener L = new WindowAdapter(){
			public void windowClosing(WindowEvent e){ System.exit(0);}
		};
		userInput.addWindowListener(L);
	}
	
	public class MyKeyboard implements KeyListener {
		public MyKeyboard() { 
		}
		public void keyTyped(KeyEvent e) { }
		public void keyPressed(KeyEvent e) {
			if (e.getKeyCode() == 40) {
				System.out.println("Velocity magnitude = " + advice[0]);
				if (advice[0] == 0){
					advice[0] = 0;
				} else {
					advice[0] = advice[0] - 10;
				}	
			}
			if (e.getKeyCode() == 37) {
				System.out.println("Direction is = " + advice[1]);
				if (advice[1] > 400){
					advice[1] = 0;
				} else {
					advice[1] = advice[1] + 1;
				}	
			}
			if (e.getKeyCode() == 38) {
				System.out.println("Velocity magnitude = " + advice[0]);
				if (advice[0] == 100) {
					advice[0] = 100;
				} else {
					advice[0] = advice[0] + 10;
				}
			}
			if (e.getKeyCode() == 39) {
				System.out.println("Direction is = " + advice[1]);
				if (advice[1] == 0){
					advice[1] = 399;
				} else {
					advice[1] = advice[1] - 1;
				}
			}
		} // End of keyPressed function
		
		public void keyReleased(KeyEvent e) { }
	} // End of class mykeyboard
	
	public class myButton implements ActionListener {
		public myButton(){
		}

		/* (non-Javadoc)
		 * @see java.awt.event.ActionListener#actionPerformed(java.awt.event.ActionEvent)
		 */
		public void actionPerformed(ActionEvent arg0) {
			System.out.println("Shutting everything down.");
		 	restart();	
		}		
	}

	/**
	 * Send a restart to the brainstem. Could be useful?
	 *
	 */
	public void restart() {
		try {
			byte[] data = { 24 };
			jPacket restart = new jPacket((byte) 2, data);
			stem.SendPacket(restart);
			Thread.sleep(100);
		} catch (Exception e) {
			if (verbose)
				e.printStackTrace();
		}
		System.exit(1);
	} // End of restart function

	public void delay(int time) {
		try {
			Thread.sleep(time);
		} catch (InterruptedException e) {
			if (verbose)
				e.printStackTrace();
		}
	} // End of delay function

	private double limitSpeed(double value) {
		if (value > 100)
			value = 100;
		if (value < -100)
			value = -100;
		return value;
	}

	public void on(double left, double right) {
		double leftActual = 0;
		double rightActual = 0;
		left = limitSpeed(left);
		right = limitSpeed(right);
		try {
			if (moveFlipDirection) {
				leftActual = 0.005 * left + 0.5;
				rightActual = -1 * 0.005 * right + 0.5;
			} else {
				leftActual = -1 * 0.005 * left + 0.5;
				rightActual = 0.005 * right + 0.5;
			}
			if (verbose)
				System.out.println(
					"Left is " + leftActual + " and right is " + rightActual);
			gp.ServoAbs((byte) 0, (float) leftActual);
			gp.ServoAbs((byte) 1, (float) rightActual);
		} catch (Exception e) {
			if (verbose)
				e.printStackTrace();
		}
	} // End of on function

	/**
	 * Function to read sensor readings. 
	 * The microcontroller must be slammed with requests to get a reading. Each reading is 
	 *  placed inside a while loop that exits only when a real reading is recieved. 
	 * @param sensor Index of sensor to read
	 * @return Scaled value of sensor reading. Return value is uController resolution * (0.0 - 1.0)
	 */
	public float readSensor(int sensor) {
		byte portID = (byte) sensor;
		float reading = 0f;
		boolean flag = false;
		//		System.out.println("I am reading a senor");
		while (!flag) {
			//			System.out.println("Help ..I am stuck in a while loop" + sensor);
			try {
				if (sensor <= 4) {
					reading = 1023 * gp.AnalogIn(portID);
				} else {
					if (sensor == LEFT_SONAR) {
						byte low = gp.PadReadByte((byte) 29);
						byte high = gp.PadReadByte((byte) 28);
						reading = Conversions.bytesToInteger(high, low);
					} // End of if statement for left sonar
					if (sensor == RIGHT_SONAR) {
						byte low = gp.PadReadByte((byte) 31);
						byte high = gp.PadReadByte((byte) 30);
						reading = Conversions.bytesToInteger(high, low);
					} // End of if statment for right sonar
				} // End of else statement
				Thread.sleep(10);
				//				System.out.println("The reading is " + reading);
				flag = true;
			} catch (Exception e) {
			}
		}
		return reading;
	}
	
	public double[] getResultant(){
		return null;
	}

	/**
	 * Threaded class to worry about sensor polling. All other processes can look to the values
	 * that this thread looks up. Eliminates stubborn sensor polling moments. (These seem to happen pretty often)
	 * @author Matt Krugman
	 *
	 */
	public class ThreadSensorPolling extends Thread {
		public volatile boolean stop = false;

		public ThreadSensorPolling() {
			super("Sensor Polling");
		} // End of constructor

		public void run() {
			if (verbose)
				System.out.println(
					"Beginning the execution of brainstem sensor polling.");
			while (!stop) {
				sensorReadings[ANALOG0] = readSensor(ANALOG0);
				sensorReadings[ANALOG1] = readSensor(ANALOG1);
				sensorReadings[ANALOG2] = readSensor(ANALOG2);
				sensorReadings[LEFT_SONAR] = readSensor(LEFT_SONAR);
				sensorReadings[RIGHT_SONAR] = readSensor(RIGHT_SONAR);

				//System.out.println(sensorReadings[ANALOG0]);
				//				try { Thread.sleep(2); } 
				//				catch (InterruptedException e) { e.printStackTrace(); }
			} // End of while loop
		} // End of run
	} // End of ThreadSensorPolling class
}