using System;
using System.Linq;
using Unity.Mathematics;
using UnityEngine;

public class FigurMitBeinenBewegung : MonoBehaviour
{
    public GameObject[] wobblingParts;
    public float[] wobblingMaxAngles;
    public bool[] wobblingInversed;
    /// <summary>
    /// Periodendauer des Wackelns wackelnder Körperteile (in Sekunden). 0, falls kein Wackeln.
    /// </summary>
    public float wobblingPeriod;
    public float movementVelocity;

    private float movementStartTime;
    private float targetPositionX;
    private bool moving;
    private bool wobbling;
    private float lastPhase;

    public void GoToXPosition(float x) {
        targetPositionX = x;
        movementStartTime = Time.time;
        moving = true;
        wobbling = true;
        if (x > transform.position.x) {
            transform.rotation = Quaternion.identity;
        } else if (x < transform.position.x) {
            transform.rotation = Quaternion.AngleAxis(180, Vector3.up);
        }
    }

    // Start is called once before the first execution of Update after the MonoBehaviour is created
    void Start()
    {
        targetPositionX = transform.position.x;
        moving = false;
        wobbling = false;
        lastPhase = 0;
    }

    // Update is called once per frame
    void Update()
    {
        if (moving){
            float deltaX = Time.deltaTime * movementVelocity;
            if (targetPositionX - transform.position.x < 0){
                deltaX *= -1;
            }
            if ((transform.position.x - targetPositionX) * (transform.position.x + deltaX - targetPositionX) <= 0){
                transform.position = new(targetPositionX, transform.position.y, transform.position.z);
                moving = false;
            } else {
                transform.position += Vector3.right * deltaX;
            }
        }
        if (wobbling && wobblingPeriod != 0){
            float phase = math.sin((Time.time - movementStartTime) * math.PI2 / wobblingPeriod);
            if (!moving && phase * lastPhase <= 0){
                wobbling = false;
                phase = 0;
            }
            lastPhase = phase;
            for (int i = 0; i < wobblingParts.Length; i++){
                float angle = phase * wobblingMaxAngles[i];
                if (wobblingInversed[i]){
                    angle *= -1;
                }
                wobblingParts[i].transform.localRotation = Quaternion.AngleAxis(angle, Vector3.forward);
            }
        }
    }
}