package imseProc.stepperMotor.controllers;

import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.io.PrintWriter;
import java.util.LinkedList;
import java.util.List;
import java.util.concurrent.ConcurrentLinkedQueue;
import java.util.regex.Matcher;
import java.util.regex.Pattern;

import oneLiners.OneLiners;

import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.NoSuchPortException;
import gnu.io.PortInUseException;
import gnu.io.SerialPort;
import gnu.io.UnsupportedCommOperationException;

/** Controller for the wierd OWIS Stepper motor controller box thing 
 * which is truly awful and has an awful protocol.
 * 
 * Hence this code is truly awful. 
 * 
 * I want to cry.
 */
public class SerialOWISDriver {
	private static final char axisChar[] = new char[]{ 'X' , 'Y', 'Z' }; 
	private String portName;
	private InputStream inStream;
	private OutputStream outStream;
	private CommsRunner reader;
	private Thread readerThread;
	private SerialPort serialPort;
	private int currentPos = -1;
	private int axis = 0;
	
	private StepperHanger hanger;
		
	private static final long minTimeToNextCmd = 10;
	private static final long updatePositionPeriod = 500;	
	
	private ConcurrentLinkedQueue<String> commandQueue = new ConcurrentLinkedQueue<String>();
	
	private List<StepperCommand> cmdSequence = null;
	private int sequenceIndex = -1;
	private long sequenceStartTime = -1;
	
	public SerialOWISDriver(StepperHanger hanger, String portName, int baudRate) {
		this.portName = portName;
		this.hanger = hanger;
		connect(portName, baudRate);
		
		reader = new CommsRunner();
		readerThread = new Thread(reader);
		readerThread.start();
		
		commandQueue.add("VD");
		//send it 'home' and reset the counter there
		//this will basically kill the box if done
		//for an axis without the switch connected.
		commandQueue.add("H"+axisChar[axis]); 
		commandQueue.add("C"+axisChar[axis]);
		
		gotoPos(0); //it should already be there due to the H					
	}
	
	public void gotoPos(long pos){
		commandQueue.add("E"+axisChar[axis]);
		commandQueue.add("A"+axisChar[axis]+"+" + pos + ",1000,0,1000");
		commandQueue.add("S"+axisChar[axis]);
	}
	
	public long getPos(){ return currentPos; }
	
	public boolean isBusy(){ return (reader.busyWithCmd != null); }
	public String getStatus(){
		return ((cmdSequence == null) ? "No Seq" : "Seq Active") +
				 ((reader.busyWithCmd != null) ? ", busy["+reader.busyWithCmd+"]" : "");
	}
		
	public void destroy(){
		reader.death = true;
		readerThread.interrupt();
		
		serialPort.close();
		System.out.println("SerialOWISDriver: Disconnected.");
	}

	private void connect(String portName, int baudRate) {

		try {
			CommPortIdentifier portIdentifier;
			portIdentifier = CommPortIdentifier.getPortIdentifier(portName);
		
			if ( portIdentifier.isCurrentlyOwned() )
				throw new RuntimeException("Error: Port is currently in use");
	
			CommPort commPort = portIdentifier.open(this.getClass().getName(), 2000);
	
			if(!(commPort instanceof SerialPort)) 
				throw new RuntimeException("Error: '"+portName+"' is not a serial port.");
	
			serialPort = (SerialPort) commPort;
			serialPort.setSerialPortParams(baudRate, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
			//serialPort.setFlowControlMode(SerialPort.FLOWCONTROL_RTSCTS_IN | SerialPort.FLOWCONTROL_RTSCTS_OUT);
			serialPort.setFlowControlMode(SerialPort.FLOWCONTROL_XONXOFF_IN | SerialPort.FLOWCONTROL_XONXOFF_OUT);
			
	
			inStream = serialPort.getInputStream();
			outStream = serialPort.getOutputStream();

			
		} catch (Exception e) {
			System.err.println("SerialOWISDriver: Unable to open serial port '"+portName+"': ");
			throw new RuntimeException(e);
		}
		
	}
	
	private void procResponse(long receiveTimeMS, String rsp){
		Pattern p = Pattern.compile(".*C"+axisChar[axis]+"[ +]*([0-9-]*).*");
		
	    Matcher m = p.matcher(rsp);
	    if(m.matches()){
	    	String posString = m.replaceAll("$1");
	    	int newPos = OneLiners.mustParseInt(posString);
	    	if(newPos != currentPos){
	    		currentPos = newPos;	    	
	    	}
		    if(hanger != null){
	    		hanger.updatePosition(receiveTimeMS, new int[]{ newPos });
		    }
	    }
	    
	}
	
	private class CommsRunner implements Runnable {
		boolean death = false;
		/** this is busy in the sense of not responding to commands
		 * When the motor is going somewhere in response to a 'AX' etc command
		 * the thing will still responsd */ 
		public String busyWithCmd = null;  
		
		@Override
		public void run() {
			try {

				//for some reason, the box terminates lines only with the LF!
				//and is incredibly flaky
				//and slow
				//and falls over a lot
				//and generally sucks
				
				StringBuffer lineBuild = new StringBuffer();
				
				outStream.write(new byte[]{ 0x0D, 0x0D }); //clear any hanging commands
				outStream.flush();

				long lastAck = 0;
				String activeCmd = "RS";
				write(activeCmd);
				String lastCmd = "";
				
				do{
					//Every command returns a ':', some commands return other stuff too
					// but after the ':', so we can never be sure what belongs to what
					
					if(inStream.available() > 0){
						long receiveTimeMS = System.currentTimeMillis(); //as close as we can get
						int byteRead = inStream.read();
						
						if(byteRead >= 0x20)						
							lineBuild.append((char)byteRead);
						else
							lineBuild.append("[0x" + Integer.toHexString(byteRead) + "]");
						
						if(activeCmd != null && byteRead == ':'){
							lastCmd = activeCmd;
							activeCmd = null;
							//System.out.println("SerialOWISDriver: Received ready following cmd '"+lastCmd+"'.");
							lastAck = System.currentTimeMillis();
							busyWithCmd = null;
				    		if(hanger != null)
				    			hanger.driverStatusChanged();
							
						}else if(byteRead == 0x0D){
							//System.out.println("SerialOWISDriver: Read line: '" + lineBuild.toString() + "', probably in response to '" +
							//		((activeCmd != null) ? activeCmd : lastCmd) + "'");
							procResponse(receiveTimeMS, lineBuild.toString());
							lineBuild.setLength(0);					
						}					
						if(byteRead < 0)
							throw new RuntimeException("SerialOWISDriver: End of stream on serial port");
						 
					}else if(activeCmd == null && (System.currentTimeMillis() - lastAck) > minTimeToNextCmd){
						activeCmd = commandQueue.poll();
						
						if(activeCmd != null){
							write(activeCmd);
							
						}else if(checkSequence()){
							//we'll catch it on the next round, 
							
						}else if((System.currentTimeMillis() - lastAck) > updatePositionPeriod){
							activeCmd = "C"+axisChar[axis];
							write(activeCmd);
						}
					}else{
						try {
							Thread.sleep(100);
						} catch (InterruptedException e) { }
					}
					
				}while(!death);
				
			} catch (IOException e) {
				throw new RuntimeException(e);
			}
			
		}
		
		/** See if we need to queue a message from the StepperCommand sequence */
		private boolean checkSequence(){
			if(cmdSequence == null)
				return false;
			
			if(sequenceStartTime < 0){ //haven't started yet
				sequenceStartTime = System.currentTimeMillis(); //so we'll start now
				sequenceIndex = 0;
			}
			
			long now = System.currentTimeMillis();
			StepperCommand nextCmd = cmdSequence.get(sequenceIndex);
			
			if((now - sequenceStartTime) < nextCmd.time)
				return false; //not yet
			
			axis = nextCmd.axis; //switch to that axis
			String cmd = "A" + axisChar[axis] + 
							((nextCmd.position>=0) ? "+" : "-") +
							nextCmd.position + "," + 
							nextCmd.speed + "," + 
							nextCmd.rampDuration + "," + 
							nextCmd.rampStartSpeed;
			
			commandQueue.add(cmd);
			commandQueue.add("S" + axisChar[axis]);
			
			sequenceIndex++;
			
			if(sequenceIndex >= cmdSequence.size()){
				stopSequence();
			}
				
			return true;			
		}
		
		private void write(String str) throws IOException{
			char c[] = str.toCharArray();
			for(int i=0; i < c.length; i++){
				outStream.write(c[i]);
				try { Thread.sleep(10); } catch (InterruptedException e) { }
			}
			busyWithCmd = str.substring(0, Math.min(2, str.length()));
    		outStream.write(0x0D);
    		outStream.flush();
			//try { Thread.sleep(100); } catch (InterruptedException e) { }
			if(hanger != null)
    			hanger.driverStatusChanged();
			
		}
		
	}

	public static void main(String[] args) {
		new SerialOWISDriver(null, "/dev/ttyS0", 9600);
	}

	public void startSequence(List<StepperCommand> cmds) {
		sequenceStartTime = -1;
		sequenceIndex = -1;
		cmdSequence = cmds;		
	}
	
	public void stopSequence(){
		cmdSequence = null;
		hanger.sequenceStopped();
		commandQueue.add("EA"); //stop all axes
		//commandQueue.add("H"+axisChar[axis]); //and return home, which is probably desired
	}
	
	public void resetHome(){
		commandQueue.add("EA"); //stop all axes
		commandQueue.add("H"+axisChar[axis]); //and return home
	}

	public boolean isSequenceActive() { return cmdSequence != null; }
}