/**********************************************************************************************
 *   UnlimitedHand for Unity Plugin ver 0033
 *   This code is used with an Arduino Code "Serial_4Unity_4Processing" in UH4Arduino0033.zip.
 *   
 *   [Notification 1]
 *   Please setup the Compatible Level to ".NET 2.0" on your Unity project to use Serial Connection.
 *   
 *   [Notification 2]
 *   If you use windos PC, Please change Port Name at Line 201: return "COM3"; : in this code.
 * 
 *   [Notification 3]
 *   A sample unity project tutorial with this plugin will be publish around 30 April.
 *   Please check http://dev.unlimitedhand.com
 * ******************************************************************************************/
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;

using System.IO;
using System.IO.Ports; 
using System.Threading;


public class UH : MonoBehaviour {
	//Serial Connection//
	public string PortName = "";		// PortName
	private SerialPort _serialPort;
	public int Baud = 115200;           // Default baud rate
    public int RebootDelay = 10;     	
    
    public bool AutoConnect = true;    	// Connect automatically 
    public bool Connected { get; private set; } // true when the device is connected

	//UH instance
    private static UH instance = null;        
    public static UH global { get { return instance; } } 
    
    public enum READ_TYPE {HIGH_SPEED, LOW_SPEED};

	//Objects: Accel and Gyro 
    public float[] UHGyro = new float[3];
    public float[] UHAccel = new float[3];  
    public float[] UHGyroAccelData = new float[7];

	//Object: Foream angle
	public int[] UHAngle = new int[3];
	public bool updateAngleFlg;

	//Object: Tempature of the UnlimitedHand  
    public float UHTmp;
    public float[] UHPos = new float[6]; // x,y,z, pitch, yaw, roll
    public int[,] calibData = new int[16,3];
	
	//Object: Photo-reflectors' value to detect hand movements
    public int[] UHPR = new int[8];  
	public bool updatePhotoSensorsFlg; 

    /////////////////////////////////////////////////////
	/// AWAKE
	/////////////////////////////////////////////////////
    void Awake () {        
         
        Debug.Log("UnlimitedHand awake");

		updateAngleFlg = true;

		updatePhotoSensorsFlg = true;

        if (instance == null) instance = this;
        
        DontDestroyOnLoad(this);
        
        if (AutoConnect){
            Debug.Log("AutoConnecting...");
            if (PortName == null || PortName.Length == 0 && UH.guessPortName().Length > 0){
                PortName = UH.guessPortName();
            }  
            Connect();                                
        }

		UHGyroAccelData [0] = 0;
		UHGyroAccelData [1] = 0;
		UHGyroAccelData [2] = 0;
		UHGyroAccelData [3] = 0;
		UHGyroAccelData [4] = 0;
		UHGyroAccelData [5] = 0;
		UHGyroAccelData [6] = 0;

		UHPR[0] = 0;
		UHPR[1] = 0;
		UHPR[2] = 0;
		UHPR[3] = 0;
		UHPR[4] = 0;
		UHPR[5] = 0;
		UHPR[6] = 0;
		UHPR[7] = 0;

    }
    
	/////////////////////////////////////////////////////
	/// UPDATE
	/////////////////////////////////////////////////////
    void Update()
    {
        if (_serialPort != null && _serialPort.IsOpen)
        {
            // process incoming serial messages    
            //            processInput();    
            //updateUH3DGyroAccel ();
			if(updateAngleFlg)updateAngle();

			if(updatePhotoSensorsFlg)updatePhotoSensors();
        }                    
    }
    
   
    List<Action> setup_queue = new List<Action>();
    private object setup_lock = new object();
    
	/////////////////////////////////////////////////////
	/// SETUP
	/////////////////////////////////////////////////////
    public void Setup(Action action)
    {
        lock(setup_lock)
        {
            if (!Connected)
                setup_queue.Add(action);        
            else
                action();
        }
    }
    
	/////////////////////////////////////////////////////
	/// SERIAL CONNECTION
	/////////////////////////////////////////////////////
    public void Connect()
    {
        Debug.Log ("Connecting to UnlimitedHand at " + PortName + "...");        

        _serialPort = new SerialPort(PortName, Baud);
        //_serialPort = Win32SerialPort.CreateInstance();
        
        _serialPort.DtrEnable = true; // win32 hack to try to get DataReceived event to fire
        _serialPort.RtsEnable = true; 
        _serialPort.PortName = PortName;
        _serialPort.BaudRate = Baud;

        _serialPort.DataBits = 8;
        _serialPort.Parity = Parity.None;
        _serialPort.StopBits = StopBits.One;
        _serialPort.ReadTimeout = 1; // since on windows we *cannot* have a separate read thread
        _serialPort.WriteTimeout = 1000;
        
        _serialPort.Open ();
        
        if (_serialPort.IsOpen)
        {
            Thread.Sleep(RebootDelay);
        }
        
    }
    
    IEnumerator setup_delay (){    
        yield return new WaitForSeconds(RebootDelay);
//        Debug.Log("Version data received, running setup commands");            
        lock(setup_lock)
        {
            Connected = true;    
            setup_queue.Clear();
        }
        
    }
    
    
    public void Disconnect()
    {
        Connected = false;
        Close ();
        
    }
    
    void OnDestroy()
    {                
        Disconnect();
    }
    
    protected void Close(){
        if (_serialPort != null) {
            _serialPort.Close ();
        }
    }
    

    // return port name
    public static string guessPortName()
    {        
        switch (Application.platform)
        {
        case RuntimePlatform.OSXPlayer:
            return guessPortNameUnix ();
        case RuntimePlatform.OSXEditor:
            return guessPortNameUnix ();
        case RuntimePlatform.OSXDashboardPlayer:
            return guessPortNameUnix ();
        case RuntimePlatform.LinuxPlayer:
            return guessPortNameUnix();
            
        default: 
            return guessPortNameWindows();
        }
    }
    
    public static string guessPortNameWindows()
    {
        var devices = System.IO.Ports.SerialPort.GetPortNames();
        
        if (devices.Length == 0) 
        {
            return "COM3";  //please write the port name in your windows enviroment    
        } else
            return devices[0];                
    }
    
    public static string guessPortNameUnix()
    {            
        var devices = System.IO.Ports.SerialPort.GetPortNames();
        
        if (devices.Length ==0) // try manual enumeration
        {
            devices = System.IO.Directory.GetFiles("/dev/");
            Debug.Log(" Read the devices");
        }
        string dev = ""; ;            
        foreach (var d in devices)
        {           
			Debug.Log (d);
    
			if(d.StartsWith ("/dev/cu.usb") || d.StartsWith ("/dev/tty.usb"))
			{
				dev  =  d;       
				Debug.Log ("Guessing that UnlimitedHand is device "  +  dev);
				return dev;
			}

        }
		foreach (var d in devices) 
		{
			if (d.StartsWith ("/dev/cu.RNBT") || d.StartsWith ("/dev/tty.RNBT")) {
				dev  =  d;     				           
				Debug.Log ("Guessing that UnlimitedHand is device "  +  dev);
				return dev;
			}
		}        
        return dev;        
    }
    
    
	/////////////////////////////////////////////////////
	/// READ Photo-reflector sensors values via Serial Connection
	/////////////////////////////////////////////////////
    public int[] readPhotoSensors(){
        return UHPR;
    }

	public void updatePhotoSensors(){
		try{
			_serialPort.WriteLine ("c"); 
			string data = _serialPort.ReadLine (); 
			//Debug.Log(data);
			if(data!=null && data.Contains("_")) {
				UHPR [0] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None) [0]);
				UHPR [1] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None) [1]);
				UHPR [2] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None) [2]);
				
				UHPR [3] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None) [3]);
				
				UHPR [4] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None)[4]);
				UHPR [5] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None)[5]);
				UHPR [6] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None)[6]);
				UHPR [7] = int.Parse (data.Split (new string[]{"_"}, System.StringSplitOptions.None)[7]);
			}
		}catch(TimeoutException){
			//UHPR[0] = 0;
			//UHPR[1] = 0;
			//UHPR[2] = 0;
			//UHPR[3] = 0;
			//UHPR[4] = 0;
			//UHPR[5] = 0;
			//UHPR[6] = 0;
			//UHPR[7] = 0;
		}

	}

	public  void updateUH3DGyroAccel(){
		try{
			_serialPort.WriteLine ("a"); 
			//Debug.Log ("Send a message a . ");
			string data = _serialPort.ReadLine (); 
			//Debug.Log (data );
			if(data!=null && data.Contains("+")) {
				UHGyroAccelData [0] = float.Parse (data.Split (new string[]{"+"}, System.StringSplitOptions.None) [0]);
				UHGyroAccelData [1] = float.Parse (data.Split (new string[]{"+"}, System.StringSplitOptions.None) [1]);
				UHGyroAccelData [2] = float.Parse (data.Split (new string[]{"+"}, System.StringSplitOptions.None) [2]);
				
				UHGyroAccelData [3] = float.Parse (data.Split (new string[]{"+"}, System.StringSplitOptions.None) [3]);
				
				UHGyroAccelData [4] = float.Parse (data.Split (new string[]{"+"}, System.StringSplitOptions.None)[4]);
				UHGyroAccelData [5] = float.Parse (data.Split (new string[]{"+"}, System.StringSplitOptions.None)[5]);
				UHGyroAccelData [6] = float.Parse (data.Split (new string[]{"+"}, System.StringSplitOptions.None)[6]);
			}
		}catch(TimeoutException){
			/*
			UHGyroAccelData[0] = 0.0f;
			UHGyroAccelData[1] = 0.0f;
			UHGyroAccelData[2] = 0.0f;
			UHGyroAccelData[3] = 0.0f;
			UHGyroAccelData[4] = 0.0f;
			UHGyroAccelData[5] = 0.0f;
			UHGyroAccelData[6] = 0.0f;
			*/
		}
		//return UHGyroAccelData;
	}
	
	public void updateAngle(){
		try{
			_serialPort.WriteLine ("A"); 
			//Debug.Log ("Send a message : A");
			string data2 = _serialPort.ReadLine (); 
			//Debug.Log(data2);
			if(data2!=null && data2.Contains("+")) {
					UHAngle [0] = int.Parse (data2.Split (new string[]{"+"}, System.StringSplitOptions.None) [0]);
					UHAngle [1] = int.Parse (data2.Split (new string[]{"+"}, System.StringSplitOptions.None) [1]);
					UHAngle [2] = int.Parse (data2.Split (new string[]{"+"}, System.StringSplitOptions.None) [2]);
				}
			}catch(TimeoutException){				
				//UHAngle[0] = 0;
				//UHAngle[1] = 0;
				//UHAngle[2] = 0;
			}
	}
	
	public float[] readUH3DAccel(){
		UHAccel [0] = UHGyroAccelData [0];
		UHAccel [1] = UHGyroAccelData [1];
		UHAccel [2] = UHGyroAccelData [2]; 
		return UHAccel;
	}  
	public float[] readGyro (){
		UHGyro [0] = UHGyroAccelData [4];
		UHGyro [1] = UHGyroAccelData [5];
		UHGyro [2] = UHGyroAccelData [6]; 
		return UHAccel;    
	}


	public float[] readUHJointAngles(int READ_TYPE){
		float[] UHJointAngles = new float[16];
		
		return UHJointAngles;
	}

	/////////////////////////////////////////////////////
	/// EMS(Electri Muscle Stimulation) functions to move the user's hand
	/////////////////////////////////////////////////////
    
	    public void stimulate(int padNum){
        try{
			_serialPort.WriteLine (padNum.ToString()); 
            
        }catch(TimeoutException){
            
        }
        
    }
	
    public void setLevelUp(){
        try{
            _serialPort.WriteLine ("h"); 
        }catch(TimeoutException){
            
        }
    }
    
	public void setLevelDown(){
		try{
			_serialPort.WriteLine ("l"); 
		}catch(TimeoutException){

	        }
	}

	/////////////////////////////////////////////////////
	/// Vibration function
	/////////////////////////////////////////////////////
    public void vibrate(){
        try{
            _serialPort.WriteLine ("b"); 
            
        }catch(TimeoutException){
            
        }
    }
  
    
    
    public float[] estimUH3Dpos(float[] UHGyro, float[] UHAccel){
        
        return UHPos;
    }
    
    public void setUH3Dpos(float[] UHGyro, float[] UHAccel){
        
    }
    
    public float readUHtemp(){
        float cels = 0.0f;
        string data = _serialPort.ReadLine ();
        cels = float.Parse (data.Split (new string[]{" "}, System.StringSplitOptions.None)[3]);
        return cels;
    }
    
 
    
    public bool UHCalibStep(int stimulationNumber, int startLevel, int READ_TYPE){
        
        return true;
    }
    
    public int[,] UHCalibFinish(){
        
        return calibData;
    }
    
    public void UHStimPush(int[,] calibData){
        
    }
    
    public void UHStimPop(){
        
    }
    
    public bool stimUHJoint(int stimJointNum, int mSec){
        
        return true;
    }
    
    public bool stimUHJoints(int[] stimJoint, int mSec){
        
        return true;
    }
    
    public bool stimUHPad(int stimPadNum, int mSec){
        
        return true;
    }

	public void setHighSpeedMode(){
		try{
	        _serialPort.WriteLine ("s"); 
        }catch(TimeoutException){  
		}
	}
} 
