using System;
using System.Collections.Generic;
using System.Text;

namespace Motion6D
{
    public class Position : IPosition
    {

        #region Fields

        protected IReferenceFrame parent;

        protected double[] own = new double[] { 0, 0, 0 };

        protected double[] position = new double[3];

        protected object parameters;

        #endregion

        #region Ctor

        protected Position()
        {
        }

        public Position(double[] position)
        {
            for (int i = 0; i < own.Length; i++)
            {
                own[i] = position[i];
            }
        }

        #endregion

        #region IPosition Members

        double[] IPosition.Position
        {
            get { return position; }
        }

        public virtual IReferenceFrame Parent
        {
            get
            {
                return parent;
            }
            set
            {
                parent = value;
            }
        }

        public virtual object Parameters
        {
            get
            {
                return parameters;
            }
            set
            {
                parameters = value;
            }
        }


        public virtual void Update()
        {
            Update(BaseFrame);
        }

        #endregion

        #region Specific Members

        protected virtual void Update(ReferenceFrame frame)
        {
            double[,] m = frame.Matrix;
            double[] p = frame.Position;
            for (int i = 0; i < p.Length; i++)
            {
                position[i] = p[i];
                for (int j = 0; j < own.Length; j++)
                {
                    position[i] += m[i, j] * own[j];
                }
            }
        }

        protected virtual ReferenceFrame BaseFrame
        {
            get
            {
                if (parent == null)
                {
                    return Motion6D.Motion6DFrame.Base;
                }
                return parent.Own;
            }
        }

        #endregion
    }
}
