VRCycling/Assets/Scripts/Logging/ViveSensorDataLogger.cs

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;
        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(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, 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 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),
            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(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,
                hmdPosition.x, hmdPosition.y, hmdPosition.z,
                hmdRotation.x, hmdRotation.y, hmdRotation.z));
        }

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

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