using UnityEngine;

namespace KindRetargeting
{
    /// <summary>
    /// 모션 데이터 필터링을 위한 클래스
    /// </summary>
    public class MotionFilter
    {
        private Vector3[] positionBuffer;
        private Quaternion[] rotationBuffer;
        private int bufferSize;
        private int currentIndex;

        public MotionFilter(int bufferSize = 5)
        {
            this.bufferSize = bufferSize;
            positionBuffer = new Vector3[bufferSize];
            rotationBuffer = new Quaternion[bufferSize];
            currentIndex = 0;
        }

        public Vector3 FilterPosition(Vector3 newPosition)
        {
            positionBuffer[currentIndex] = newPosition;
            Vector3 smoothedPosition = Vector3.zero;

            for (int i = 0; i < bufferSize; i++)
            {
                smoothedPosition += positionBuffer[i];
            }

            currentIndex = (currentIndex + 1) % bufferSize;
            return smoothedPosition / bufferSize;
        }

        public Quaternion FilterRotation(Quaternion newRotation)
        {
            rotationBuffer[currentIndex] = newRotation;
            Vector4 average = Vector4.zero;

            for (int i = 0; i < bufferSize; i++)
            {
                if (Quaternion.Dot(rotationBuffer[0], rotationBuffer[i]) < 0)
                {
                    rotationBuffer[i] = new Quaternion(-rotationBuffer[i].x, -rotationBuffer[i].y,
                                                    -rotationBuffer[i].z, -rotationBuffer[i].w);
                }
                average.x += rotationBuffer[i].x;
                average.y += rotationBuffer[i].y;
                average.z += rotationBuffer[i].z;
                average.w += rotationBuffer[i].w;
            }

            average /= bufferSize;
            currentIndex = (currentIndex + 1) % bufferSize;
            return new Quaternion(average.x, average.y, average.z, average.w).normalized;
        }

        public void UpdateSettings(bool useFilter, bool useRoughMotion)
        {
            // 필요한 경우 여기에 설정 업데이트 로직 추가
        }
    }

    public class RoughMotion
    {
        private Vector3 lastPosition;
        private Quaternion lastRotation;
        private float smoothSpeed;

        public RoughMotion(float smoothSpeed = 5f)
        {
            this.smoothSpeed = smoothSpeed;
            lastPosition = Vector3.zero;
            lastRotation = Quaternion.identity;
        }

        public Vector3 ProcessPosition(Vector3 newPosition)
        {
            lastPosition = Vector3.Lerp(lastPosition, newPosition, smoothSpeed * Time.deltaTime);
            return lastPosition;
        }

        public Quaternion ProcessRotation(Quaternion newRotation)
        {
            lastRotation = Quaternion.Lerp(lastRotation, newRotation, smoothSpeed * Time.deltaTime);
            return lastRotation;
        }

        public void SetSmoothSpeed(float speed)
        {
            smoothSpeed = speed;
        }
    }
}