using UnityEngine;
using System.Collections;
using System;
using System.Runtime.InteropServices;
using System.Collections.Generic;

public class SimpleShapeManipulation : HapticClassScript {

    public GameObject pen;
    public GameObject setUp;
    public GameObject workPlaneA;
    public Plane workPlane;
    public GameObject hapCam;
    DrawShapes drawer;

    float stiff = .5f;
    float pop = .5f;
    float statFric = .2f;

    float x0 = 0;
    float y0 = 0;
    float z0 = 0;
    float oldx3 = 0;
    float oldy3 = 0;
    float oldz3 = 0;
    float oldx4 = 0;
    float oldy4 = 0;
    float oldz4 = 0;
    float x3 = 0;
    float y3 = 0;
    float z3 = 0;
    float x4 = 0;
    float y4 = 0;
    float z4 = 0;
    public static List<GameObject> Cubes = new List<GameObject>();
    public static List<GameObject> Spheres = new List<GameObject>();
    public static List<GameObject> thickLines = new List<GameObject>();
    public static List<GameObject> thickPolylines = new List<GameObject>();
    public static List<GameObject> thickPolylinesEnds = new List<GameObject>();
    public static List<Vector3> Polylines = new List<Vector3>();
    public static List<List<Vector3>> thinLines = new List<List<Vector3>>();
    bool switched = false;
    public static bool firstOne = true;

    //Generic Haptic Functions
    private GenericFunctionsClass myGenericFunctionsClassScript;


    //private float[] pt1 = new float[3];
    //private float[] pt2 = new float[3];
    //private float[] pt3 = new float[3];
    Vector3 pt1;
    Vector3 pt2;
    Vector3 pt3;
    private float[] myWSSize = new float[3];
    private float[] myWSPosition = new float[3];
    /*****************************************************************************/

    void Awake()
	{
		myGenericFunctionsClassScript = transform.GetComponent<GenericFunctionsClass>();
    }

    void Start()
    {
        drawer = (DrawShapes)GameObject.Find("MyWork").GetComponent(typeof(DrawShapes));
        if (PluginImport.InitHapticDevice())
		{
			Debug.Log("OpenGL Context Launched");
			Debug.Log("Haptic Device Launched");


            myGenericFunctionsClassScript.SetHapticWorkSpace();
			myGenericFunctionsClassScript.GetHapticWorkSpace();
            //Update Workspace as function of camera
            PluginImport.UpdateWorkspace(myHapticCamera.transform.rotation.eulerAngles.y);
			
			//Set Mode of Interaction
			/*
			 * Mode = 0 Contact
			 * Mode = 1 Manipulation - So objects will have a mass when handling them
			 * Mode = 2 Custom Effect - So the haptic device simulate vibration and tangential forces as power tools
			 * Mode = 3 Puncture - So the haptic device is a needle that puncture inside a geometry
			 */
			PluginImport.SetMode(ModeIndex);
			//Show a text descrition of the mode
			myGenericFunctionsClassScript.IndicateMode();

			//Set the touchable face(s)
			PluginImport.SetTouchableFace(ConverterClass.ConvertStringToByteToIntPtr(TouchableFace));
			
		}
		else
			Debug.Log("Haptic Device cannot be launched");

		/***************************************************************/
		//Set Environmental Haptic Effect
		/***************************************************************/
			// Viscous Force Example 
			myGenericFunctionsClassScript.SetEnvironmentViscosity ();

			// Constant Force Example - We use this environmental force effect to simulate the weight of the cursor 
			myGenericFunctionsClassScript.SetEnvironmentConstantForce();

			// Spring Force Example
			//myGenericFunctionsClassScript.SetEnvironmentSpring();

			// Friction Force Example
			//myGenericFunctionsClassScript.SetEnvironmentFriction();

		/***************************************************************/
		//Setup the Haptic Geometry in the OpenGL context 
		//And read haptic characteristics
		/***************************************************************/
		myGenericFunctionsClassScript.SetHapticGeometry();

		//Get the Number of Haptic Object
		//Debug.Log ("Total Number of Haptic Objects: " + PluginImport.GetHapticObjectCount());

		/***************************************************************/
		//Launch the Haptic Event for all different haptic objects
		/***************************************************************/
		PluginImport.LaunchHapticEvent();
	}
    // When added to an object, draws colored rays from the
    // transform position.

    static Material lineMaterial;
    static void CreateLineMaterial()
    {
        if (!lineMaterial)
        {
            // Unity has a built-in shader that is useful for drawing
            // simple colored things.
            Shader shader = Shader.Find("Hidden/Internal-Colored");
            lineMaterial = new Material(shader);
            lineMaterial.hideFlags = HideFlags.HideAndDontSave;
            // Turn on alpha blending
            lineMaterial.SetInt("_SrcBlend", (int)UnityEngine.Rendering.BlendMode.SrcAlpha);
            lineMaterial.SetInt("_DstBlend", (int)UnityEngine.Rendering.BlendMode.OneMinusSrcAlpha);
            // Turn backface culling off
            lineMaterial.SetInt("_Cull", (int)UnityEngine.Rendering.CullMode.Off);
            // Turn off depth writes
            lineMaterial.SetInt("_ZWrite", 0);
        }
    }

    // Will be called after all regular rendering is done
    public void OnRenderObject()
    {
        CreateLineMaterial();
        // Apply the line material
        lineMaterial.SetPass(0);

        GL.PushMatrix();
        // Set transformation matrix for drawing to
        // match our transform
        GL.MultMatrix(transform.localToWorldMatrix);
        // Draw lines
        GL.Begin(GL.LINES);
        GL.Color(Color.red);
        //print("line count: " + thinLines.Count);
        if (thinLines.Count > 0)
        {
            for (int j = 0; j < thinLines.Count; j++)
            {
                if (thinLines[j].Count > 1)
                {
                    for (int i = 1; i < thinLines[j].Count; ++i)
                    {
                        GL.Vertex3(thinLines[j][i - 1][0], thinLines[j][i - 1][1], thinLines[j][i - 1][2]);
                        GL.Vertex3(thinLines[j][i][0], thinLines[j][i][1], thinLines[j][i][2]);
                    }
                }
            }
        }
        if (Polylines.Count > 1)
        {
            for (int i = 1; i < Polylines.Count; ++i)
            {
                GL.Vertex3(Polylines[i - 1][0], Polylines[i - 1][1], Polylines[i - 1][2]);
                GL.Vertex3(Polylines[i][0], Polylines[i][1], Polylines[i][2]);
            }
        }
        GL.End();
        GL.PopMatrix();
    }

    void Update()
	{
        /***************************************************************/
        //Update Workspace as function of camera
        /***************************************************************/


        myGenericFunctionsClassScript.UpdateGraphicalWorkspace();


        



        if (PluginImport.GetButton1State())
        {
            if (SelectTool.selected.Equals("DrawThinLines"))
            {
                if (!switched) { thinLines.Add(new List<Vector3>()); switched = true; print("new lines!"); }
                float x = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                float y = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                float z = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];


                thinLines[thinLines.Count - 1].Add(new Vector3(x, y, z));
            }
            else if (SelectTool.selected.Equals("Polylines"))
            {
                float x2 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                float y2 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                float z2 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];

                if (!switched)
                {
                    float x = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                    float y = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                    float z = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];
                    Polylines.Add(new Vector3(x, y, z));
                    print("added!");
                    switched = true;
                }

                else Polylines[Polylines.Count - 1].Set(x2, y2, z2);
            }
            else if (SelectTool.selected.Equals("Cube"))
            {
                if (!switched)
                {
                    x0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                    y0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                    z0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];
                    Cubes.Add(GameObject.CreatePrimitive(PrimitiveType.Cube));
                }
                float x1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                float y1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                float z1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];

                print("0: " + x0 + ", " + y0 + ", " + z0);
                print("1: " + x1 + ", " + y1 + ", " + z1);

                Cubes[Cubes.Count - 1].transform.position = new Vector3((x0 + x1) / 2, (y0 + y1) / 2, (z0 + z1) / 2);
                Cubes[Cubes.Count - 1].transform.localScale = new Vector3(x0 - x1, y0 - y1, z0 - z1);


                if (!switched)
                {
                    Cubes[Cubes.Count - 1].tag = "Touchable";
                    Cubes[Cubes.Count - 1].AddComponent<HapticProperties>();
                    Cubes[Cubes.Count - 1].GetComponent<HapticProperties>().stiffness = stiff;
                    Cubes[Cubes.Count - 1].GetComponent<HapticProperties>().popThrough = pop;
                    Cubes[Cubes.Count - 1].GetComponent<HapticProperties>().staticFriction = statFric;
                }

                myGenericFunctionsClassScript.SetHapticGeometry();
                switched = true;
            }
            else if (SelectTool.selected.Equals("Sphere"))
            {
                if (!switched)
                {
                    x0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                    y0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                    z0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];
                    Spheres.Add(GameObject.CreatePrimitive(PrimitiveType.Sphere));
                }
                float x1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                float y1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                float z1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];

                print("0: " + x0 + ", " + y0 + ", " + z0);
                print("1: " + x1 + ", " + y1 + ", " + z1);

                Spheres[Spheres.Count - 1].transform.position = new Vector3((x0 + x1) / 2, (y0 + y1) / 2, (z0 + z1) / 2);
                Spheres[Spheres.Count - 1].transform.localScale = new Vector3(x0 - x1, y0 - y1, z0 - z1);

                if (!switched)
                {
                    Spheres[Spheres.Count - 1].tag = "Touchable";
                    Spheres[Spheres.Count - 1].AddComponent<HapticProperties>();
                    Spheres[Spheres.Count - 1].GetComponent<HapticProperties>().stiffness = stiff;
                    Spheres[Spheres.Count - 1].GetComponent<HapticProperties>().popThrough = pop;
                    Spheres[Spheres.Count - 1].GetComponent<HapticProperties>().staticFriction = statFric;
                }

                myGenericFunctionsClassScript.SetHapticGeometry();
                switched = true;
            }
            else if (SelectTool.selected.Equals("Line"))
            {
                float x1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                float y1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                float z1 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];
                if (!switched)
                {
                    x0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                    y0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                    z0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];
                    thickLines.Add(GameObject.CreatePrimitive(PrimitiveType.Cylinder));
                }

                print("0: " + x0 + ", " + y0 + ", " + z0);
                print("1: " + x1 + ", " + y1 + ", " + z1);

                thickLines[thickLines.Count - 1].transform.position = new Vector3((x0 + x1) / 2, (y0 + y1) / 2, (z0 + z1) / 2);
                thickLines[thickLines.Count - 1].transform.up = new Vector3(x0 - x1, y0 - y1, z0 - z1);
                float len = (new Vector3(x0, y0, z0) - new Vector3(x1, y1, z1)).magnitude;
                thickLines[thickLines.Count - 1].transform.localScale = new Vector3(.02f, len / 2, .02f);

                if (!switched)
                {
                    thickLines[thickLines.Count - 1].tag = "Touchable";
                    thickLines[thickLines.Count - 1].AddComponent<HapticProperties>();
                    thickLines[thickLines.Count - 1].GetComponent<HapticProperties>().stiffness = stiff;
                    thickLines[thickLines.Count - 1].GetComponent<HapticProperties>().popThrough = pop;
                    thickLines[thickLines.Count - 1].GetComponent<HapticProperties>().staticFriction = statFric;
                }

                myGenericFunctionsClassScript.SetHapticGeometry();
                switched = true;
            }
            else if (SelectTool.selected.Equals("ThickPolylines"))
            {

                if (!switched)
                {
                    thickPolylines.Add(GameObject.CreatePrimitive(PrimitiveType.Cylinder));
                    if (firstOne)
                    {
                        x0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                        y0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                        z0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];

                        firstOne = false;
                    }
                    else
                    {
                        x0 = oldx3;
                        y0 = oldy3;
                        z0 = oldz3;


                        thickPolylinesEnds.Add(GameObject.CreatePrimitive(PrimitiveType.Sphere));
                        thickPolylinesEnds[thickPolylinesEnds.Count - 1].transform.position = new Vector3(x0, y0, z0);
                        thickPolylinesEnds[thickPolylinesEnds.Count - 1].transform.localScale = new Vector3(.02f, .02f, .02f);

                    }
                }


                x3 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                y3 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                z3 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];



                print("0: " + x0 + ", " + y0 + ", " + z0);
                print("1: " + x3 + ",thickPolylines " + y3 + ", " + z3);

                if (thickPolylines.Count > 0)
                {
                    thickPolylines[thickPolylines.Count - 1].transform.position = new Vector3((x0 + x3) / 2, (y0 + y3) / 2, (z0 + z3) / 2);
                    thickPolylines[thickPolylines.Count - 1].transform.up = new Vector3(x0 - x3, y0 - y3, z0 - z3);
                    float len = (new Vector3(x0, y0, z0) - new Vector3(x3, y3, z3)).magnitude;
                    thickPolylines[thickPolylines.Count - 1].transform.localScale = new Vector3(.02f, len / 2, .02f);



                    if (!switched)
                    {
                        thickPolylines[thickPolylines.Count - 1].tag = "Touchable";
                        thickPolylines[thickPolylines.Count - 1].AddComponent<HapticProperties>();
                        thickPolylines[thickPolylines.Count - 1].GetComponent<HapticProperties>().stiffness = stiff;
                        thickPolylines[thickPolylines.Count - 1].GetComponent<HapticProperties>().popThrough = pop;
                        thickPolylines[thickPolylines.Count - 1].GetComponent<HapticProperties>().staticFriction = statFric;
                    }

                    myGenericFunctionsClassScript.SetHapticGeometry();
                    switched = true;
                }
            }
            else if (SelectTool.selected.Equals("CubeFromSameCorner"))
            {
                if (!switched)
                {
                    if (firstOne)
                    {
                        x0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                        y0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                        z0 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];
                        Cubes.Add(GameObject.CreatePrimitive(PrimitiveType.Cube));


                        firstOne = false;
                    }
                    else
                    {
                        Cubes.Add(GameObject.CreatePrimitive(PrimitiveType.Cube));
                        x0 = oldx4;
                        y0 = oldy4;
                        z0 = oldz4;

                    }
                }
                x4 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[0];
                y4 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[1];
                z4 = myGenericFunctionsClassScript.myHapticClassScript.hapticCursor.transform.position[2];

                print("0: " + x0 + ", " + y0 + ", " + z0);
                print("1: " + x4 + ", " + y4 + ", " + z4);

                Cubes[Cubes.Count - 1].transform.position = new Vector3((x0 + x4) / 2, (y0 + y4) / 2, (z0 + z4) / 2);
                Cubes[Cubes.Count - 1].transform.localScale = new Vector3(x0 - x4, y0 - y4, z0 - z4);

                if (!switched)
                {
                    Cubes[Cubes.Count - 1].tag = "Touchable";
                    Cubes[Cubes.Count - 1].AddComponent<HapticProperties>();
                    Cubes[Cubes.Count - 1].GetComponent<HapticProperties>().stiffness = stiff;
                    Cubes[Cubes.Count - 1].GetComponent<HapticProperties>().popThrough = pop;
                    Cubes[Cubes.Count - 1].GetComponent<HapticProperties>().staticFriction = statFric;
                }

                myGenericFunctionsClassScript.SetHapticGeometry();
                switched = true;
            }
        }
        else
        {
            if (switched && !SelectTool.selected.Equals("Move"))
            {
                switched = false;
                if (Cubes.Count > 0) foreach (GameObject obj in Cubes) obj.GetComponent<HapticProperties>().fixedObj = true;
                if (Spheres.Count > 0) foreach (GameObject obj in Spheres) obj.GetComponent<HapticProperties>().fixedObj = true;
                if (thickLines.Count > 0) foreach (GameObject obj in thickLines) obj.GetComponent<HapticProperties>().fixedObj = true;
                if (thickPolylines.Count > 0) foreach (GameObject obj in thickPolylines) obj.GetComponent<HapticProperties>().fixedObj = true;
                if (thickPolylinesEnds.Count > 0) foreach (GameObject obj in thickPolylinesEnds) obj.GetComponent<HapticProperties>().fixedObj = true;
            }

            oldx3 = x3;
            oldy3 = y3;
            oldz3 = z3;

            oldx4 = x4;
            oldy4 = y4;
            oldz4 = z4;
        }



        /***************************************************************/
        //Haptic Rendering Loop
        /***************************************************************/
        PluginImport.RenderHaptic ();
		
		myGenericFunctionsClassScript.GetProxyValues();

		myGenericFunctionsClassScript.GetTouchedObject();
        

		//Debug.Log ("Button 1: " + PluginImport.GetButton1State());
		//Debug.Log ("Button 2: " + PluginImport.GetButton2State());
	}

	void OnDisable()
	{
		if (PluginImport.HapticCleanUp())
		{
			Debug.Log("Haptic Context CleanUp");
			Debug.Log("Desactivate Device");
			Debug.Log("OpenGL Context CleanUp");
		}
	}

	

}
