using System;
using UnityEngine;
using ImstkUnity;

public class Rigid : DynamicalModel
{
    public double mass = 1.0;

    public Vector3[] inertia = new Vector3[3] {
            new Vector3(1.0f, 0.0f, 0.0f),
            new Vector3(0.0f, 1.0f, 0.0f),
            new Vector3(0.0f, 0.0f, 1.0f)
            };

    private Imstk.PbdBody _body;
    private Imstk.PbdModel _model;

    protected override void OnImstkInit()
    {
        imstkObject = InitObject();
        SimulationManager.sceneManager.getActiveScene().addSceneObject(imstkObject);
        InitGeometry();
        //InitGeometryMaps();
        ProcessBoundaryConditions(gameObject.GetComponents<BoundaryCondition>());
        Configure();
    }

    protected override Imstk.CollidingObject InitObject()
    {
        Imstk.PbdObject pbdObject = new Imstk.PbdObject(GetFullName());
        _model = SimulationManager.pbdModel;
        pbdObject.setDynamicalModel(_model);
        
        return pbdObject;
    }

    protected override void Configure()
    {
        Imstk.PbdBody pbdBody = (imstkObject as Imstk.PbdObject).getPbdBody();
        _body = pbdBody;

        var position = new Imstk.Vec3d(transform.position.x, transform.position.y, transform.position.z);

        var orientation = new Imstk.Quatd(transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w);
        Imstk.Mat3d inertiaTensor = Imstk.Mat3d.Identity();
        inertiaTensor.setValue(0, 0, inertia[0][0]);
        inertiaTensor.setValue(0, 1, inertia[0][1]);
        inertiaTensor.setValue(0, 2, inertia[0][1]);

        inertiaTensor.setValue(1, 0, inertia[1][0]);
        inertiaTensor.setValue(1, 1, inertia[1][1]);
        inertiaTensor.setValue(1, 2, inertia[1][2]);

        inertiaTensor.setValue(2, 0, inertia[2][0]);
        inertiaTensor.setValue(2, 1, inertia[2][1]);
        inertiaTensor.setValue(2, 2, inertia[2][2]);

        pbdBody.setRigid(
                position,
                mass,
                orientation,
                inertiaTensor
            );
    }

    public void Update()
    {
        if (_body == null) return;
        Imstk.Vec3d bodyPosition = _body.getRigidPosition();
        Imstk.Quatd bodyOrientation = _body.getRigidOrientation();

        transform.SetPositionAndRotation(bodyPosition.ToUnityVec(), bodyOrientation.ToUnityQuat());
    }

    protected override void InitGeometry()
    {
        // Copy all the geometries over to imstk, set the transform and
        // apply later. (to avoid applying transform twice *since two
        // geometries could point to the same one*)

        // No Visual Geometry
        
        // Setup the collision geometry
        if (collisionGeomFilter != null)
        {
            Imstk.Geometry colGeom = GetCollidingGeometry();
            imstkObject.setCollidingGeometry(colGeom);
        }
        else
        {
            Debug.LogError("No collision geometry provided to DynamicalModel on object " + gameObject.name);
        }

        // Setup the physics geometry
        if (physicsGeomFilter != null)
        {
            Imstk.Geometry physicsGeom = GetPhysicsGeometry();
            (imstkObject as Imstk.DynamicObject).setPhysicsGeometry(physicsGeom);
            (imstkObject as Imstk.DynamicObject).getDynamicalModel().setModelGeometry(physicsGeom);
        }
        else
        {
            Debug.LogError("No physics geometry provided to PbdRigidModel on object " + gameObject.name);
        }
    }

}

