using System;
using System.Collections.Generic;
using System.Globalization;
using System.Linq;
using Tracking;
using UnityEngine;

namespace Logging
{
    public readonly struct ViveSensorDataLog
    {
        private readonly float timestamp;
        private readonly float steerAngle;
        private readonly float frontWheelTrackerPositionX;
        private readonly float frontWheelTrackerPositionY;
        private readonly float frontWheelTrackerPositionZ;
        private readonly float frontWheelTrackerRotationX;
        private readonly float frontWheelTrackerRotationY;
        private readonly float frontWheelTrackerRotationZ;
        private readonly float kineticTrackerPositionX;
        private readonly float kineticTrackerPositionY;
        private readonly float kineticTrackerPositionZ;
        private readonly float kineticTrackerRotationX;
        private readonly float kineticTrackerRotationY;
        private readonly float kineticTrackerRotationZ;

        public ViveSensorDataLog(float timestamp, float steerAngle, float frontWheelTrackerPositionX,
            float frontWheelTrackerPositionY, float frontWheelTrackerPositionZ, float frontWheelTrackerRotationX,
            float frontWheelTrackerRotationY, float frontWheelTrackerRotationZ, float kineticTrackerPositionX,
            float kineticTrackerPositionY, float kineticTrackerPositionZ, float kineticTrackerRotationX,
            float kineticTrackerRotationY, float kineticTrackerRotationZ)
        {
            this.timestamp = timestamp;
            this.steerAngle = steerAngle;
            this.frontWheelTrackerPositionX = frontWheelTrackerPositionX;
            this.frontWheelTrackerPositionY = frontWheelTrackerPositionY;
            this.frontWheelTrackerPositionZ = frontWheelTrackerPositionZ;
            this.frontWheelTrackerRotationX = frontWheelTrackerRotationX;
            this.frontWheelTrackerRotationY = frontWheelTrackerRotationY;
            this.frontWheelTrackerRotationZ = frontWheelTrackerRotationZ;
            this.kineticTrackerPositionX = kineticTrackerPositionX;
            this.kineticTrackerPositionY = kineticTrackerPositionY;
            this.kineticTrackerPositionZ = kineticTrackerPositionZ;
            this.kineticTrackerRotationX = kineticTrackerRotationX;
            this.kineticTrackerRotationY = kineticTrackerRotationY;
            this.kineticTrackerRotationZ = kineticTrackerRotationZ;
        }


        public string[] Serialize() => new[]
        {
            timestamp.ToString("F6", CultureInfo.InvariantCulture),
            steerAngle.ToString("F6", CultureInfo.InvariantCulture),
            frontWheelTrackerPositionX.ToString("F6", CultureInfo.InvariantCulture),
            frontWheelTrackerPositionY.ToString("F6", CultureInfo.InvariantCulture),
            frontWheelTrackerPositionZ.ToString("F6", CultureInfo.InvariantCulture),
            frontWheelTrackerRotationX.ToString("F6", CultureInfo.InvariantCulture),
            frontWheelTrackerRotationY.ToString("F6", CultureInfo.InvariantCulture),
            frontWheelTrackerRotationZ.ToString("F6", CultureInfo.InvariantCulture),
            kineticTrackerPositionX.ToString("F6", CultureInfo.InvariantCulture),
            kineticTrackerPositionY.ToString("F6", CultureInfo.InvariantCulture),
            kineticTrackerPositionZ.ToString("F6", CultureInfo.InvariantCulture),
            kineticTrackerRotationX.ToString("F6", CultureInfo.InvariantCulture),
            kineticTrackerRotationY.ToString("F6", CultureInfo.InvariantCulture),
            kineticTrackerRotationZ.ToString("F6", CultureInfo.InvariantCulture),
        };
    }

    public class ViveSensorDataLogger : SensorDataLogger<ViveSensorDataLog>
    {
        public FrontWheelTracker fwt;
        public KineticLegTracker klt;

        public override string Key => "vive_sensor_data";

        private void Update()
        {
            var steerAngle = fwt.SteerRotation;
            var fwtPosition = fwt.RelativePosition;
            var fwtRotation = fwt.RelativeRotation;
            var kltPosition = klt.RelativePosition;
            var kltRotation = klt.RelativeRotation;
            //TODO: data always there?
            Log(new ViveSensorDataLog(Time.time, steerAngle,
                fwtPosition.x, fwtPosition.y, fwtPosition.z, 
                fwtRotation.x, fwtRotation.y, fwtRotation.z,
                kltPosition.x, kltPosition.y, kltPosition.z, 
                kltRotation.x, kltRotation.y, kltRotation.z));
        }

        public override void Log(ViveSensorDataLog value)
        {
            Buffer.Add(value.Serialize());
        }

        public override IEnumerable<ViveSensorDataLog> ReadLog(IEnumerable<IEnumerable<string>> lines)
        {
            throw new System.NotImplementedException();
        }
    }
}