using System;
using System.Collections.Generic;
using System.Text;
using System.Runtime.Serialization;

using CategoryTheory;
using DataPerformer;

namespace Motion6D
{
    [Serializable()]
    public class RelativeMeasurements : ISerializable, ICategoryObject, IMeasurements, IPostSetArrow
    {

        #region Fields

        const Double a = 0;

        private object obj;

        private IPosition source;


        private IPosition target;

        private IVelocity vSource;

        private IVelocity vTarget;

        private IOrientation oSource;

        private IOrientation oTarget;

        private IAngularVelocity aSource;

        private IAngularVelocity aTarget;


        private double[] relative = new double[3];


        private List<IMeasure> measurements = new List<IMeasure>();

        private bool isUpdated;

        private double distance;

        private double[] relVelocity = new double[3];

        private double velocity;

        #endregion

        #region Ctor

        public RelativeMeasurements()
        {
        }

        private RelativeMeasurements(SerializationInfo info, StreamingContext context)
        {
        }

        #endregion

        #region ISerializable Members

        void ISerializable.GetObjectData(SerializationInfo info, StreamingContext context)
        {
            
        }

        #endregion

        #region ICategoryObject Members

        ICategory ICategoryObject.Category
        {
            get { throw new Exception("The method or operation is not implemented."); }
        }

        ICategoryArrow ICategoryObject.Id
        {
            get { throw new Exception("The method or operation is not implemented."); }
        }

        #endregion

        #region IPostSetArrow Members

        void IPostSetArrow.PostSetArrow()
        {
            SetMeasurements();
        }

        #endregion

        #region IAssociatedObject Members

        object IAssociatedObject.Object
        {
            get
            {
                return obj;
            }
            set
            {
                obj = value;
            }
        }

        #endregion

        #region IMeasurements Members

        int IMeasurements.Count
        {
            get { return measurements.Count; }
        }

        IMeasure IMeasurements.this[int n]
        {
            get { return measurements[n]; }
        }

        void IMeasurements.UpdateMeasurements()
        {
            double[] y = source.Position;
            double[] x = target.Position;
            double dist = 0;
            for (int i = 0; i < 3; i++)
            {
                double dd = y[i] - x[i];
                dist += dd * dd;
                relative[i] = dd;
            }
            distance = Math.Sqrt(dist);
            if (vSource != null && vTarget != null)
            {
                double[] Vy = vSource.Velocity;
                double[] Vx = vTarget.Velocity;
                velocity = 0;
                for (int i = 0; i < 3; i++)
                {
                    double v = Vy[i] - Vx[i];
                    relVelocity[i] = v;
                    velocity += v * relative[i];
                }
                velocity /= distance;
            }
        }

        bool IMeasurements.IsUpdated
        {
            get
            {
                return isUpdated;
            }
            set
            {
                isUpdated = value;
            }
        }

        #endregion

        #region Specific Members


        public IPosition Source
        {
            set
            {
                if (source != null & value != null)
                {
                    throw new Exception("Souce already exists");
                }
                source = value;
                getParameters(source, ref vSource, ref oSource, ref aSource);
                update();
            }
        }


        public IPosition Target
        {
            set
            {
                if (target != null & value != null)
                {
                    throw new Exception("Target already exists");
                }
                target = value;
                getParameters(target, ref vTarget, ref oTarget, ref aTarget);
                update();
            }
        }

        void SetMeasurements()
        {
            measurements.Clear();
            MeasureParameter mpd = getDistance;
            getParameters(target, ref vTarget, ref oTarget, ref aTarget);
            getParameters(source, ref vSource, ref oSource, ref aSource);
            if (vSource == null | vTarget == null)
            {
                IMeasure md = new Measure(a, mpd, "Distance");
                measurements.Add(md);
            }
            else
            {
                MeasureParameter mpv = getVelocity;
                IMeasure mdv = new MeasureDerivation(a, mpd, mpv, "Distance");
                measurements.Add(mdv);
                measurements.Add(new Measure(a, mpv, "Radial"));
            }
        }

        object getDistance()
        {
            return distance;
        }

        object getVelocity()
        {
            return velocity;
        }


        void getParameters(IPosition p, ref IVelocity velocity, ref IOrientation orientation, ref IAngularVelocity om)
        {
            if (p == null)
            {
                velocity = null;
                orientation = null;
                om = null;
            }
            IPosition pa = p;
            if (p is IReferenceFrame)
            {
                IReferenceFrame f = p as IReferenceFrame;
                pa = f.Own;
            }
            if (pa is IVelocity)
            {
                velocity = pa as IVelocity;
            }
            else
            {
                velocity = null;
            }
            if (pa is IOrientation)
            {
                orientation = pa as IOrientation;
            }
            else
            {
                orientation = null;
            }
            if (pa is IAngularVelocity)
            {
                om = pa as IAngularVelocity;
            }
            else
            {
                om = null;
            }
        }

        private void update()
        {
            if (source == null | target == null)
            {
                measurements.Clear();
                return;
            }
            SetMeasurements();
        }


 
        #endregion

    }
}
