using System;
using System.Collections.Generic;
using System.Globalization;
using Logging.Base;
using Tracking;
using UnityEngine;

namespace Logging.Data
{
    public readonly struct FrontWheelTrackerData : ISerializable
    {
        private readonly long timestamp;
        private readonly float debugLegTrackerPositionX;
        private readonly float debugLegTrackerPositionY;
        private readonly float debugLegTrackerPositionZ;
        private readonly float debugPosOnBikePlaneX;
        private readonly float debugPosOnBikePlaneY;
        private readonly float debugPosOnBikePlaneZ;
        private readonly float debugFrontWheelTrackerRelativePositionX;
        private readonly float debugFrontWheelTrackerRelativePositionY;
        private readonly float debugFrontWheelTrackerRelativePositionZ;

        public FrontWheelTrackerData(long timestamp, float debugLegTrackerPositionX, float debugLegTrackerPositionY,
            float debugLegTrackerPositionZ, float debugPosOnBikePlaneX, float debugPosOnBikePlaneY,
            float debugPosOnBikePlaneZ, float debugFrontWheelTrackerRelativePositionX,
            float debugFrontWheelTrackerRelativePositionY, float debugFrontWheelTrackerRelativePositionZ)
        {
            this.timestamp = timestamp;
            this.debugLegTrackerPositionX = debugLegTrackerPositionX;
            this.debugLegTrackerPositionY = debugLegTrackerPositionY;
            this.debugLegTrackerPositionZ = debugLegTrackerPositionZ;
            this.debugPosOnBikePlaneX = debugPosOnBikePlaneX;
            this.debugPosOnBikePlaneY = debugPosOnBikePlaneY;
            this.debugPosOnBikePlaneZ = debugPosOnBikePlaneZ;
            this.debugFrontWheelTrackerRelativePositionX = debugFrontWheelTrackerRelativePositionX;
            this.debugFrontWheelTrackerRelativePositionY = debugFrontWheelTrackerRelativePositionY;
            this.debugFrontWheelTrackerRelativePositionZ = debugFrontWheelTrackerRelativePositionZ;
        }

        public KeyValuePair<long, string[]> Serialize()
        {
            return new KeyValuePair<long, string[]>(timestamp, new[]
            {
                debugLegTrackerPositionX.ToString("F4", CultureInfo.InvariantCulture),
                debugLegTrackerPositionY.ToString("F4", CultureInfo.InvariantCulture),
                debugLegTrackerPositionZ.ToString("F4", CultureInfo.InvariantCulture),
                debugPosOnBikePlaneX.ToString("F4", CultureInfo.InvariantCulture),
                debugPosOnBikePlaneY.ToString("F4", CultureInfo.InvariantCulture),
                debugPosOnBikePlaneZ.ToString("F4", CultureInfo.InvariantCulture),
                debugFrontWheelTrackerRelativePositionX.ToString("F4", CultureInfo.InvariantCulture),
                debugFrontWheelTrackerRelativePositionY.ToString("F4", CultureInfo.InvariantCulture),
                debugFrontWheelTrackerRelativePositionZ.ToString("F4", CultureInfo.InvariantCulture)
            });
        }
    }

    public class FrontWheelTrackerLogger : SensorDataLogger<FrontWheelTrackerData>
    {
        public FrontWheelTracker frontWheelTracker;
        public override string Key => "front_wheel_tracker";

        private void Update()
        {
            var legTrackerPos = frontWheelTracker.legTracker.RelativePosition;
            var posOnBikePlane = frontWheelTracker.DebugPosOnBikePlane;
            var fwtRelativePos = frontWheelTracker.RelativePosition;
            Log(new FrontWheelTrackerData(Helpers.RoundToLong(Time.time * 1000f),
                legTrackerPos.x,
                legTrackerPos.y,
                legTrackerPos.z,
                posOnBikePlane.x,
                posOnBikePlane.y,
                posOnBikePlane.z,
                fwtRelativePos.x,
                fwtRelativePos.y,
                fwtRelativePos.z));
        }

        public override IEnumerable<FrontWheelTrackerData> ReadLog(IEnumerable<IEnumerable<string>> lines)
        {
            throw new NotImplementedException(); //TODO
        }
    }
}