using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.XR;
using UnityEngine.XR.Interaction.Toolkit;

public class ArmControl : MonoBehaviour
{
    public GameObject Rcontroller;
    public XRController controller;
    public float l1;
    public float l2;
    public float l3;
    public float acc;
    public float datx = 0;
    public float daty = 0;
    public float datz = 0;
    private Vector3 pos;
    private Vector3 rot;
    public float posx;
    public float posy;
    public float posz;
    public float pitch;
    public float length;
    public float lengthp;
    public float a1;
    public float a2;
    public float a3;
    public float a4;
    private float yaw;
    public int baseval;
    public int elbow;
    public int shoulder;
    public int wrist;
    private float grip;
    public int gripper;
    public float test1;
    

    void Start()
    {
        
    }

    // Update is called once per frame
    void Update()
    {

        pos = Rcontroller.transform.position;
        rot = Rcontroller.transform.rotation.eulerAngles;
        posx = pos.x;
        posy = pos.y;
        posz = -pos.z;
        if (controller.inputDevice.TryGetFeatureValue(CommonUsages.primaryButton, out bool button) && button)
        {
            datx = pos.x - (float)0.15/acc;
            daty = pos.y - (float)0.05/acc;
            datz = -pos.z;
        }
        
        length = Mathf.Sqrt(Mathf.Pow(acc*(posx - datx),2) + Mathf.Pow(acc*(posy - daty),2) + Mathf.Pow(acc*(posz - datz),2));
        lengthp = Mathf.Sqrt(Mathf.Pow(acc*(posx - datx),2) + Mathf.Pow(acc*(posz - datz),2));
        a4 = 180 * Mathf.Asin((posy-daty) / length) / Mathf.PI;
        a1 = 180 * Mathf.Acos((Mathf.Pow(l1,2) + Mathf.Pow(length,2)- Mathf.Pow(l2,2)) / (2 * l1 * length)) / Mathf.PI;
        a3 = 180 * Mathf.Acos((Mathf.Pow(l1,2) + Mathf.Pow(l2,2)- Mathf.Pow(length,2)) / (2 * l1 * l2)) / Mathf.PI;
        a2 = 180 * Mathf.Acos((Mathf.Pow(l2,2) + Mathf.Pow(length,2) - Mathf.Pow(l1,2)) / (2 * l2 * length)) / Mathf.PI;
        yaw = -180 * Mathf.Asin(acc*(posz - datz) / lengthp) / Mathf.PI;
        controller.inputDevice.TryGetFeatureValue(CommonUsages.trigger, out grip);

        test1 = Mathf.Pow(posx-datx,2);

        if (rot.x < 180)
        {
            pitch = -rot.x-(float)60;
        }
        if (rot.x > 180)
        {
            pitch = 300 - rot.x;
        }

        shoulder = Mathf.RoundToInt(a1 + a4);
        baseval = Mathf.RoundToInt(90 + yaw);
        elbow = Mathf.RoundToInt(a3 - 90);
        wrist = Mathf.RoundToInt(90 + a2 - a4);
        gripper = Mathf.RoundToInt(100 * grip + 10);

        
        
    }

    public int Getgripper()
    {
        return gripper;
    }

    public int Getbase()
    {
        return baseval;
    }

    public int Getelbow()
    {
        return elbow;
    }
    
    public int Getwrist()
    {
        return wrist;
    }

    public int Getshoulder()
    {
        return shoulder;
    }
}
