using AnyCAD.Foundation;

namespace AnyCAD.Robot
{
    public class RobotModel
    {
        RobotBody mRobot;
        ParticleSceneNode mTrackingNode = new ParticleSceneNode(1000, Vector3.Red, 5);
        uint nSeedIdx = 0;

        public RobotBody Robot { get { return mRobot; } }
        public ParticleSceneNode TrackingPath { get {return mTrackingNode;} }

        public RobotModel(RobotBody robot)
        {
            mRobot = robot;
            mTrackingNode.SetVisible(false);
        }

        public string Name { get=>mRobot.GetName(); set=>mRobot.SetName(value); }
        public void Reset()
        {
            mRobot.Reset();
        }

        public bool ShowTracking { get => mTrackingNode.GetVisible(); set=>mTrackingNode.SetVisible(value); }

        bool mShowLink = true;
        public bool ShowLinks { get => mShowLink; set { mRobot.ShowLinks(value); mShowLink = value; }  }

        bool mShowVirtualLinks = true;
        public bool ShowVLinks { get => mShowVirtualLinks; set { mRobot.ShowVirtualLinks(value); mShowVirtualLinks = value; } }

        public void AddTrackingPoint(Vector3 position)
        {
            mTrackingNode.SetPosition(nSeedIdx++, position);
            mTrackingNode.UpdateBoundingBox();
            mTrackingNode.RequstUpdate();
        }

        public double GetVariable(uint armIdx, uint idx)
        {
            return mRobot.GetVariable(armIdx, idx);
        }

        public Matrix4 GetFinalPosition(uint armIdx)
        {
            return mRobot.GetFinalTransform(armIdx);
        }
        public double AddVariable(uint armIdx, uint idx, double step)
        {
            double val = mRobot.GetVariable(armIdx, idx);
            val += step;
            mRobot.SetVariable(armIdx, idx, val);
         
            mRobot.UpdateFrames();

            return val;
        }
    }
}