using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Leg : MonoBehaviour {
    public Transform Hip1;
    public Transform Hip2;
    public Transform Knie;

    public double a1 = 0;
    public double a2 = 0;
    public double a3 = 0;

    public Vector3 point = new Vector3(10, 10, 0);
    public Quaternion rotation = new Quaternion(0, 0, 0, 0);

    public Vector3 transformation = new Vector3(0, 0, 0);
    public Vector3 transformation2 = new Vector3(0, 0, 0);

    public bool invert = false;

    public double translateX = 0;
    public double translateY = 0;
    public double translateZ = 0;

    public GameObject pointShouldBe;
    public Vector3 pos = new Vector3(0, 0, 0);

    public int startServo = -1;

    private Foot foot = new Foot();

    // Use this for initialization
    void Start () {
		
	}

    // Update is called once per frame
    public void FixedUpdate()
    {
        foot.X = point.x;
        foot.Y = point.y;
        foot.Z = point.z;

        foot.rotationX = rotation.x;
        foot.rotationY = rotation.y;
        foot.rotationZ = rotation.z;

        foot.translateX = translateX;
        foot.translateY = translateY;
        foot.translateZ = translateZ;

        // transform before rotation.
        foot.transformation = transformation;

        // transform after rotation.
        foot.transformation2 = transformation2;

        foot.invert = invert;

        foot.setleg();

        if (!invert)
        {
            a1 = foot.O1 - 90;
            a2 = 90 - foot.O2;
            a3 = foot.O3 - 90;
        }
        else
        {
            a1 = (foot.O1 - 90)*-1;
            a2 = 90 - foot.O2;
            a3 = foot.O3 - 90;
        }

        if (!invert && startServo != -1)
        {
            Serial.Instance.send(startServo,
                (int)Remap((float)a1, -90, 90, 2500, 500),
                (int)Remap((float)a2, -90, 90, 2500, 500), // 0 is middel
                (int)Remap((float)a3, 90, -90, 2500, 500)); // 0 is middel
        }
        else if (invert && startServo != -1)
        {
            double a1 = foot.O1 - 90;
            double a2 = 90 - foot.O2;
            double a3 = foot.O3 - 90;

            Serial.Instance.send(startServo,
                (int)Remap((float)a1, -90, 90, 500, 2500),
                (int)Remap((float)a2, -90, 90, 500, 2500), // 0 is middel
                (int)Remap((float)a3, 90, -90, 500, 2500)); // 0 is middel
        }

        Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0);
        Hip2.transform.localRotation = Quaternion.Euler((float)a2, 0, 0);
        Knie.transform.localRotation = Quaternion.Euler((float)a3, 0, 0);

        pointShouldBe.transform.localPosition = foot.pos;
    }

    public static float Remap(float value, float from1, float to1, float from2, float to2)
    {
        return (value - from1) / (to1 - from1) * (to2 - from2) + from2;
    }
}