package edu.wmich.cs.nest.agora.vis.qor;

 

import java.io.*;
import java.util.*;
import javax.comm.*;

public class PortRead implements SerialPortEventListener, Runnable {

    static CommPortIdentifier portId;
     static Enumeration portList;
     InputStream inputStream;
     SerialPort serialPort;
    
      private Thread updateThread;    
      String buffer = new String ();//global
      String remains = new String ("S#"); 
      double [] var = new double[6];
      int updateInterval;//=1000 ;
      double angle_x = 0.0 , angle_y = 0.0 , angle_z =0.0;
      int j=0;

  public PortRead(CommPortIdentifier portId,Enumeration portList,int interval) {
      
         this.portId=portId;
         this.portList=portList;
         
         try {
           	 serialPort = (SerialPort) portId.open("SimpleReadApp", 2000);
        } catch (PortInUseException e) {
            
            System.out.println(e);
        }
        try {
            	inputStream = serialPort.getInputStream();
        } catch (IOException e) {
            
            System.out.println(e);
        }
        try {
	  serialPort.addEventListener(this);
        } catch (TooManyListenersException e) {}
        serialPort.notifyOnDataAvailable(true);
        try {
         	   serialPort.setSerialPortParams(9600,
                	   SerialPort.DATABITS_8,
                     SerialPort.STOPBITS_1,
                     SerialPort.PARITY_NONE);
        } catch (UnsupportedCommOperationException e) {}
 
	
	updateInterval= interval;
                  start();

    }



    public void serialEvent(SerialPortEvent event) {
	
        switch(event.getEventType()) {
       	 case SerialPortEvent.BI:
        	case SerialPortEvent.OE:
        	case SerialPortEvent.FE:
        	case SerialPortEvent.PE:
        	case SerialPortEvent.CD:
        	case SerialPortEvent.CTS:
        	case SerialPortEvent.DSR:
        	case SerialPortEvent.RI:
        	case SerialPortEvent.OUTPUT_BUFFER_EMPTY:
            	break;
        	case SerialPortEvent.DATA_AVAILABLE:
             	byte[] readBuffer = new byte[16];            			// buffer size depends upon baud rate
	
            try {
                		while (inputStream.available() > 0) {
                    		int numBytes = inputStream.read(readBuffer);
                		}

		String str = new String(readBuffer);
		buffer =buffer.concat(str.trim());
		buffer=buffer.trim(); 

              } catch (IOException e) {}
              break;
        }
    }
    
    
    public void run() { 
        while ( true ) { 
        	    try {   
                		Thread.sleep( updateInterval );  
            	   }  
            	   catch (InterruptedException e ) { } 
       recalculate(); 
        } 
    } 
 
    public void start() {
        System.out.println("STARTING PORT READ START!!!");
   	     if ( updateThread == null ) { 
            		updateThread = new Thread(this); 
	           	try {   
               			 Thread.sleep( 30000);  //to leave random values generated initially, the sensor is kept stationary during this time 
                			  buffer = "";
            		}  
            		catch (InterruptedException e ) { } 
          		updateThread.start(); 
 	       } 
    } 
 

    public void recalculate (){
    int k=0,i=0,m,n; 
    double ang_rate;
    double var_max=0;
    
    double [] acc_bias= {  -0.050505050505050324,-0.06531591575579826,-0.1613199443111468};   // acc_bias has been calculated keeping the sensor on a plane perpendicular to g , bias tend to drift with time and temperature changes 												
    double [] w_bias = {464.27899096385534,464.27899096385534,481.9277108433734};
    double[] mean =new double[6];
    double[] lasttime=new double[6];
   
    String[] temp = buffer.split("S",2);
    String temp_buffer = new String (temp[1]);
     buffer="";

     StringTokenizer st1 = new StringTokenizer(remains+temp_buffer,"S");
     int K=st1.countTokens();
     double [][] sensor_read = new double[6][K+4];
     double [][] time = new double[6][K+4];
     double [][] angle = new double[3][K+4];
     angle[0][0]=angle_x;
     angle[1][0]=angle_y;
     angle[2][0]=angle_z;

     StringTokenizer st = new StringTokenizer(remains+temp_buffer,"#");                                                 // 6 sensor readings are separated by a '#' and the sets of 6 readings are separated using 'S' 
     int l = st.countTokens();
     while (l>1) {
 	String data = new String(st.nextToken()); 
	String[] value = data.split(",",2); 					             // each reading consists of the (sensor value,current_time)
                  if (value[0].equals("S") || (value[0].contains("S"))){			             // 'S' indicates starting of a set of 6 degrees of freedom
  		i=0; k++;
  		//System.out.println(sensor_read[0][k-1]+"    "+sensor_read[1][k-1]+"    "+sensor_read[2][k-1]);//+"\n"+sensor_read[2][k-1]+"    "+sensor_read[4][k-1]+"    "+sensor_read[5][k-1]);
	}
			
	else if(!(data.equals(value[0]))&&value[0].matches("\\d+") && value[1].matches("\\d+.{1}\\d+") ){    //to remove bad data 
		int Value=Math.min(675,Integer.parseInt(value[0]));                    //sensor outputs max 3.3v on each axis so output data should be 675 at max                
		time[i][k]=Double.parseDouble(value[1])-lasttime[i]; 
		time[i][0]=0.0;			
		if (i<2)     {
			sensor_read[i][k] = ((Value/1023.0*5)-1.65)/0.330+acc_bias[i];      //output of each axis is 3.3V full scale, 1024 is maximum possible output from adc with ref voltage of 5V , sensitivity of accelerometer is 0.33 V/g 							
			sensor_read[i][k] =sensor_read[i][k]-Math.sin(angle[i][k]*Math.PI/180); //removing the gravity component 
			if(j==0){
				angle[i][k]=Math.asin(sensor_read[i][k])*180/Math.PI;  //calculating the initial angle 
				if (i==1) j++;
			}	
		}
		else if(i==2) {
			sensor_read[i][k] = ((Value/1023.0*5)-1.65)/0.330+acc_bias[i];
			sensor_read[i][k]=sensor_read[i][k]-(Math.cos(angle[0][k]*Math.PI/180)*Math.cos(angle[1][k]*Math.PI/180));
		}			
		else if (i>=3&&k>0)  {       
			ang_rate= ((Value/1023.0*5)-1.65)/0.00083+w_bias[i-3] ;        // sensitivity of gyro is 0.83mV/degree/sec
			sensor_read[i][k]=ang_rate*time[i][k]/1000;
			angle[i-3][k]=angle[i-3][k-1]+sensor_read[i][k];	
		
		}
				
		lasttime[i]=Double.parseDouble(value[1]);
		mean[i]=mean[i]+sensor_read[i][k];
		 i++ ;
				
	}
	else if(k>0&&i>0){
		time[i][k]=time[i][k-1];
		sensor_read[i][k]=sensor_read[i][k-1];
		lasttime[i]=lasttime[i-1];
		if(i>2) angle[i-3][k]=angle[i-3][k-1]+sensor_read[i][k];
		mean[i]=mean[i]+sensor_read[i][k];
		i++;
	}
	else{
		time[i][k]=0.0;
		sensor_read[i][k]=0.0;
		i++;
	}
	if(i>=6) i=0;
	//System.out.println(sensor_read[i][k]+","+time[i][k]);
	if(k>0 &&time[i][k-1]>0 && 2>=(time[i][k]/time[i][k-1]))    // to remove bad data
		 time[i][k]=time[i][k-1];
	
	l--;
         }
        angle_x=angle[0][k]; angle_y=angle[1][k] ; angle_z=angle[2][k];
        if(temp_buffer.lastIndexOf('#')==temp_buffer.length()-1)
	        remains = st.nextToken()+"#";
        else
 	       remains = st.nextToken();

		
       for (m=0; m<6;m++){
                  mean[m]=mean[m]/k;
	for(n=0; n<=k;n++ )
		var[m]=var[m]+Math.pow((mean[m]-sensor_read[m][n]),2);
	if(m<3)
		var[m]=Math.min((var[m]/k)/15*10,10);              // scaling var values between 0-10
	else
		var[m]=Math.min(Math.min(var[m]/k,1000)/1000*10,10);
	var_max=Math.max(var_max, var[m]);
	
      }		
       //System.out.println(var_max);
       mdot.Main.getMe().setObjectAsDouble(structures.Constants.QOR_VARIANCE, var_max);
    } 
    
    
}

