using System;
using System.Collections.Generic;
using Controller.Bicycle;
using Controller.Lean;
using Study;
using Sensors;
using Sensors.ANT;
using Sensors.Bluetooth;
using Tracking;
using System.Linq;
using UnityEngine;

// Declare outisde for public visibility
//public enum SteeringMode { frontWheel, Leaning, HMD };

namespace Controller
{
    [Serializable]
    public struct FrontWheelTrackerConfig
    {
        public FrontWheelTracker frontWheelTracker;
        public float multiplicator;

        public float AdjustedRotation => frontWheelTracker.SteerRotation * multiplicator;
    }

    [Serializable]
    public struct HMDTrackerConfig
    {
        public HMDTracker cameraTracker;
        public float multiplicator;
        public float steeringOffset;

        public float AdjustedRotation => cameraTracker.SteerRotation * multiplicator;
        public float AdjustedLean => cameraTracker.LeanRotation * multiplicator;
    }

    [RequireComponent(typeof(IBicycleController))]
    public class SensorBikeController : MonoBehaviour
    {
        public PolarRotationMapping polarRotationMapping;
        public FrontWheelTrackerConfig frontWheelTrackerConfig;
        public HMDTrackerConfig hmdTrackerConfig;
        private float leanFactor;

        public bool steer = true;
        public bool accelerate = true;
        public bool lean = true;
        public Study.SteeringMode steeringSelection;

        private IBicycleController bicycleController;
        private bool isFrontWheelTrackerNotNull;
        private bool isHMDTrackerNotNull;
        private BikeSensorData sensorData;

        private Queue<float> previousLeanValues;
        public int framesToConsider = 10;
        public float tolerance_global = 5;
        public float tolerance_local = 7;
        public int claibrationcount = 10;

        private void Start()
        {
            isFrontWheelTrackerNotNull = frontWheelTrackerConfig.frontWheelTracker != null;
            isHMDTrackerNotNull = hmdTrackerConfig.cameraTracker != null;
            bicycleController = GetComponent<IBicycleController>();
            sensorData = BikeSensorData.Instance;
            //leanFactor = 90f / (polarRotationMapping.maxRight - polarRotationMapping.center);
            leanFactor = 30f / polarRotationMapping.maxRight; //von 45 auf 30 angepasst
            if (framesToConsider > 1) previousLeanValues = new Queue<float>(framesToConsider);

            var polarData = sensorData.BleData;
            if (steeringSelection == Study.SteeringMode.Leaning && polarData != null)
            {
                CalibratePolar()
                //polarRotationMapping.center = polarData.Value.Acc.y;
            }
            Debug.Log("Default fpr previousLeanValues " + previousLeanValues.DefaultIfEmpty());
        }

        private void CalibratePolar()
        {
            Debug.Log("Calibration of Polar Sensor");
            Queue<float> calibrationLeanValues;
            calibrationLeanValues = new Queue<float>(claibrationcount);
            for (int i = 0; i < claibrationcount; i++)
            {
                float pos = sensorData.BleData.Value.Acc.y;
                calibrationLeanValues.Enqueue(pos);
                yield return new WaitForSeconds(0.25f); //evtl. new loeschen
            }
            polarRotationMapping.center = calibrationLeanValues.Average();
            
        }

        private void Update()
        {
            Debug.Log("Bike Sensor Controller called");
            var speedData = sensorData.SpeedData;

            if (speedData != null && accelerate) SetSpeed(speedData.Value);

            if (steer) SetSteer();

            if (lean) SetLeaningAngle();
        }

        public void SetCondition(Study.SteeringMode mode)
        {
            Debug.Log("Setting Condition");
            steeringSelection = mode;
        }

        private void SetSteer()
        {
            switch (steeringSelection)
            {
                case Study.SteeringMode.frontWheel:
                    if (isFrontWheelTrackerNotNull)
                    {
                        bicycleController.CurrentSteerAngle = frontWheelTrackerConfig.AdjustedRotation * 2f;
                    }
                    break;
                case Study.SteeringMode.Leaning:
                    var polarData = sensorData.BleData;
                    if (polarData != null)
                    {
                        Debug.Log("Updating Polar Data");
                        Debug.Log("Polar Y: " + polarData.Value.Acc);
                        var polarLean = CalculateLeanRotationMultiFrame(polarData);
                        Debug.Log("Ploar Post-Processed: " + polarLean);
                        bicycleController.CurrentSteerAngle = polarLean;
                        // Activate below in case we also need the steering angle
                        //bicycleController.CurrentSteerAngle = (polarData.Value.Acc.y - polarRotationMapping.center) * leanFactor;
                    }
                    break;
                case Study.SteeringMode.HMD:
                    if (isHMDTrackerNotNull)
                    {
                        // We emprirically observed a right-drift
                        // We subtract a constant to counteract this
                        bicycleController.CurrentSteerAngle = hmdTrackerConfig.AdjustedRotation;
                    }
                    break;
            }
            Debug.Log("Updating Steering Angle to " + bicycleController.CurrentSteerAngle);
        }

        private void SetLeaningAngle()
        {
            //bicycleController.CurrentLeaningAngle = 
        }

        private void SetSpeed(SpeedSensorData speedData)
        {
            bicycleController.CurrentSpeed = speedData.Speed;
        }

        // Helpers for Lean Steering
        private float CalculateLeanRotationSingleFrame(BleSensorData? polarData)
        {
            float steerUpdate = polarData.Value.Acc.y - polarRotationMapping.center;
            //lokal average:
            //steerUpdate = Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance_local ? 0 : steerUpdate;
            //steerUpdate = Math.Pow(steerUpdate - polarRotationMapping.center, 2) < 30 ? 0 : steerUpdate;
            return steerUpdate * leanFactor;
        }


        private float CalculateLeanRotationMultiFrame(BleSensorData? polarData)
        {
            var angleThisFrame = CalculateLeanRotationSingleFrame(polarData);
            if (previousLeanValues.Count > framesToConsider) previousLeanValues.Dequeue();

            previousLeanValues.Enqueue(angleThisFrame);

            //neue-Option:
            //float steerOld = previousLeanValues.Peek();
            //float mean = previousLeanValues.Average();
            //steerUpdate = Math.Pow(steerUpdate - mean, 2) < tolerance_global ? steerOld : steerUpdate; 

            //alte-Option:

            // Use exponential moving average
            float alpha = 2f / (framesToConsider + 1);
            float steerUpdate = previousLeanValues.DefaultIfEmpty(polarRotationMapping.center) //angepasst: Default ist nun center
             .Aggregate((ema, nextQuote) => alpha * nextQuote + (1 - alpha) * ema);

            // Do Global Thresholding, to avoid noisy steering
            return Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance_global ? 0 : steerUpdate;
            //return steerUpdate;
        }
    }
}