/*=========================================================================

   Library: iMSTK-Unity

   Copyright (c) Kitware, Inc. 

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0.txt

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

=========================================================================*/

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace ImstkUnity
{
    /// <summary>
    /// Set ups multiple grasping components, handles grasping and upgrasping
    /// this set of components. This alleviates the burden to the user to set 
    /// up multiple grasping components. Plays together with the Grasping Controller
    /// that handles the User interaction side of making a tool that can grasp, but
    /// can also be used on its own
    /// </summary>
    public class GraspingManager : MonoBehaviour
    {
        [Serializable]
        public struct GraspingData
        {
            public bool enabled;
            public DynamicalModel target;
            public Grasping.GraspType type;
        }

        public Rigid rigid;
        public GeometryFilter graspingGeometry;
        public List<GraspingData> graspingData = new List<GraspingData>();

        List<Grasping> _graspings = new List<Grasping>();
        Coroutine _coroutine;
        
        public double deformableGraspingStiffness = 0.4;
        public double rigidGraspingStiffness = 1 / 0.0001;
        
        public
        void Awake()
        {
            // These will be created _before_ the simulation manager searches for 
            // its component and therefor will participate in the norm imstk-unity
            // discovery
            foreach (var item in graspingData )
            {
                if (item.enabled)
                {
                    var grasping = gameObject.AddComponent<Grasping>();
                    grasping.rigidModel = rigid;
                    grasping.graspingGeometry = graspingGeometry;
                    grasping.graspedObject = item.target;
                    grasping.graspType = item.type;
                    grasping.deformableGraspingStiffness = deformableGraspingStiffness;
                    grasping.rigidGraspingStiffness = rigidGraspingStiffness;
                    _graspings.Add(grasping);
                }
            }
        }

        /// <summary>
        /// Sets off a corouting to go through all of the graspings that 
        /// this component created, as it takes a frame to check wether
        /// a grasp was successful
        /// </summary>
        public void StartGrasp()
        {
            _coroutine = StartCoroutine(CoroutineGrasp());
        }

        private IEnumerator CoroutineGrasp()
        {
            foreach (var grasp in _graspings)
            {
                if (!grasp.enabled) continue;

                // It takes a frame for us to detect if the grasp
                // has generated constraints, only then do we know
                // that something has _actually_ been taken
                grasp.StartGrasp();

                yield return null;

                if (grasp.HasConstraints())
                {
                    yield break;
                }
            }
        }
        public void EndGrasp()
        {
            if (_coroutine != null) {
                StopCoroutine(_coroutine);
                _coroutine = null;
            }
            foreach (var grasp in _graspings)
            {
                grasp.EndGrasp();
            }
        }

    }

}