VRCycling/Assets/Scripts/Logging/Data/ViveSensorDataLogger.cs

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

namespace Logging.Data
{
    public readonly struct ViveSensorDataLog : ISerializable
    {
        private readonly long 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;
        private readonly float hmdPositionX;
        private readonly float hmdPositionY;
        private readonly float hmdPositionZ;
        private readonly float hmdRotationX;
        private readonly float hmdRotationY;
        private readonly float hmdRotationZ;

        public ViveSensorDataLog(long 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, float hmdPositionX, float hmdPositionY,
            float hmdPositionZ, float hmdRotationX, float hmdRotationY, float hmdRotationZ)
        {
            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;
            this.hmdPositionX = hmdPositionX;
            this.hmdPositionY = hmdPositionY;
            this.hmdPositionZ = hmdPositionZ;
            this.hmdRotationX = hmdRotationX;
            this.hmdRotationY = hmdRotationY;
            this.hmdRotationZ = hmdRotationZ;
        }


        public KeyValuePair<long, string[]> Serialize()
        {
            return new KeyValuePair<long, string[]>
            (timestamp,
                new[]
                {
                    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),
                    hmdPositionX.ToString("F6", CultureInfo.InvariantCulture),
                    hmdPositionY.ToString("F6", CultureInfo.InvariantCulture),
                    hmdPositionZ.ToString("F6", CultureInfo.InvariantCulture),
                    hmdRotationX.ToString("F6", CultureInfo.InvariantCulture),
                    hmdRotationY.ToString("F6", CultureInfo.InvariantCulture),
                    hmdRotationZ.ToString("F6", CultureInfo.InvariantCulture)
                }
            );
        }
    }

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

        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;
            var hmdPosition = hmd.position;
            var hmdRotation = hmd.rotation.eulerAngles;
            //TODO: data always there?
            Log(new ViveSensorDataLog(Helpers.RoundToLong(Time.time * 1000), 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,
                hmdPosition.x, hmdPosition.y, hmdPosition.z,
                hmdRotation.x, hmdRotation.y, hmdRotation.z));
        }

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