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

   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 UnityEngine;

namespace ImstkUnity
{
    /// <summary>
    /// This class is used to enable grasping between a rigid object and a deformable.
    /// Assigning a rigid and a deformable, then use <c>StartGrasp()</c> and <c>EndGrasp()</c>
    /// to drive the grasping. <c>useVertexGrasping</c> indicates that the grasped item on
    /// the deformable will be a vertex, otherwise it will be a whole cell (e.g. triangle)
    /// When grasped iMSTK will attempt to maintain the relation between the grasped and the
    /// grasping geometry. When the rigid is moved the opposite side will attempt to move with
    /// it.
    /// </summary>
    /// 
    public class Grasping : ImstkInteractionBehaviour
    {
        public Rigid rigidModel;
        public Deformable pbdModel;

        public bool useVertexGrasping = true;

        Imstk.PbdObjectGrasping interaction;
        private Imstk.AnalyticalGeometry graspingGeometry;
        private StandardCollisionTypes collisionDetectionType = StandardCollisionTypes.Auto;

        bool OneIsA<T>(DynamicalModel a, DynamicalModel b) where T : DynamicalModel
        {
            if (b as T != null) return true;
            if (a as T != null) return true;
            return false;
        }

        public override Imstk.SceneObject GetImstkInteraction()
        {
            if (rigidModel == null || pbdModel == null)
            {
                Debug.LogError("Both models need to be assigned for the interaction to work");
            }

            collisionDetectionType = CollisionInteraction.GetCDAutoType(rigidModel, pbdModel);

            if (collisionDetectionType == StandardCollisionTypes.Auto)
            {
                Debug.LogError("Could not determine collision type for grasping in " + gameObject.name);
            }

            interaction = new Imstk.PbdObjectGrasping(pbdModel.GetDynamicObject() as Imstk.PbdObject,
                rigidModel.GetDynamicObject() as Imstk.PbdObject);

            Imstk.Geometry geom = rigidModel.GetDynamicObject().getCollidingGeometry();
 
            Imstk.AnalyticalGeometry analytical = Imstk.Utils.CastTo<Imstk.AnalyticalGeometry>(geom);

            if (analytical == null)
            {
                Debug.LogError("Can't convert to analytical geometry" + geom.getTypeName());
            }

            return interaction;
        }

        public void StartGrasp()
        {
            Imstk.Geometry geom = rigidModel.GetDynamicObject().getCollidingGeometry();
            Imstk.AnalyticalGeometry analytical = Imstk.Utils.CastTo<Imstk.AnalyticalGeometry>(geom);

            if (analytical == null)
            {
                Debug.LogError("Grasping Geometry can't be null in " + gameObject.name);
                return;
            }
            if (useVertexGrasping) interaction.beginVertexGrasp(analytical);
            else interaction.beginCellGrasp(analytical, collisionDetectionType.ToString());
        }

        public void EndGrasp()
        {
            interaction.endGrasp();
        }

        public bool HasConstraints()
        {
            return interaction.hasConstraints();
        }
    }

}
