using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Microsoft.MixedReality.Toolkit.Input;
using Microsoft.MixedReality.Toolkit;

namespace RosSharp.RosBridgeClient
{
    public class GripperStrengthPublisher : UnityPublisher<MessageTypes.Std.Float64>
    {
        private MessageTypes.Std.Float64 message;

        protected override void Start()
        {
            base.Start();
            InitializeMessage();
        }

        private void InitializeMessage()
        {
            message = new MessageTypes.Std.Float64
            {
                data = 0.0f
            };
        }

        private void FixedUpdate()
        {
            // Listening to all controller inputs detected by MRTK
            foreach (var controller in CoreServices.InputSystem.DetectedControllers)
            {
                // Interactions for a controller is the list of inputs that this controller exposes
                foreach (MixedRealityInteractionMapping inputMapping in controller.Interactions)
                {
                    // Isolate Thumbstick signal
                    if (inputMapping.Description == "Trackpad-Thumbstick Position")
                    {
                        // Small deadzone
                        if (inputMapping.Vector2Data.y > 0.05f)
                        {
                            // Convert input signal to gripper control signal
                            message.data = inputMapping.Vector2Data.y * 0.5;
                            Publish(message);
                            break;
                        }
                    }

                }
            }
        }
    }
}