// Joseph Garcia

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Media.Media3D;
using System.Runtime.CompilerServices;

namespace MEMS_Visualizer 
{
    public class VisualizationRenderer
    {
        // hand and thumb constant points
        private Point3D handBase = new Point3D(0, 0.5, 4);
        private Point3D handTip = new Point3D(0, 0.5, 0);
        //private Point3D handBase = new Point3D(-0.051795, 0.000000, 5.022323);
        //private Point3D handTip = new Point3D(-0.051795, 0.000000, -5.043309);
        private readonly Point3D thumbBase = new Point3D(-2.5, .5, 3.5);
        private readonly Point3D thumbTip = new Point3D(-2.5, .5, 0);

        // constant animation transition time
        private readonly Duration animationTime = new Duration(TimeSpan.FromSeconds(0.5));

        // models
        private GeometryModel3D handModel = new GeometryModel3D();
        private GeometryModel3D thumbModel = new GeometryModel3D();
        private Model3DGroup modelGroup = new Model3DGroup(); // holds hand and thumb models and lights
        private ModelVisual3D modelVisual3D = new ModelVisual3D(); // holds modelGroup
        
        // hand and thumb rotations
        private RotateTransform3D modelRotation = new RotateTransform3D();
        private RotateTransform3D thumbRotation = new RotateTransform3D();

        // Storyboard lists hold rotation animation sequence
        private List<Storyboard> handRotationAnimations;
        private List<Storyboard> thumbRotationAnimations;

        // movement data
        private MovementData movementData { get; set; }

        // index of current rotation animation
        public int currentRotationIndex { get; set; }

        // viewport holds rendered scene
        private Viewport3D viewport = new Viewport3D();
        public Viewport3D Viewport
        {
            get
            {
                return viewport;
            }
        }

        // count of animations
        public int TotalAnimations
        {
            get
            {
                return handRotationAnimations.Count;
            }
        }
        
        // current hand rotation
        public Vector3D CurrentHandRotation
        {
            get
            {
                if (currentRotationIndex == 0)
                    return new Vector3D(0, 0, 0);
                else
                    return movementData.HandRotationSequence[currentRotationIndex - 1];
            }
        }

        // current thumb position
        public double CurrentThumbPosition
        {
            get
            {
                if (currentRotationIndex == 0)
                    return 0.0;
                else
                    if (movementData.IsVectorProduct)
                        return 0;
                    else
                        return movementData.ThetaSequence[currentRotationIndex - 1];
            }
        }

        // current thumb rotation
        public Vector3D CurrentThumbRotation
        {
            get
            {
                if (currentRotationIndex == 0)
                    return new Vector3D(0, 0, 0);
                else
                    return movementData.ThumbRotationSequence[currentRotationIndex - 1];
            }
        }
        
        // constructor requires MovementData as an argument
        public VisualizationRenderer(MovementData movementData)
        {
            startRendering(movementData);
        }
        private void startRendering(MovementData movementData)
        {
            this.movementData = movementData;
            
            // initialize animation Storyboards
            handRotationAnimations = new List<Storyboard>(movementData.HandRotationSequence.Count);
            thumbRotationAnimations = new List<Storyboard>(movementData.HandRotationSequence.Count);

            // Register dependency property names for use in animations
            NameScope.SetNameScope(viewport, new NameScope());
            viewport.RegisterName("modelRotation", modelRotation);
            viewport.RegisterName("thumbRotation", thumbRotation);

            // initialize index of current rotation animation
            currentRotationIndex = 0;
            
            // set up 3D scene (camera / lighting / models)
            initializeScene();

            // apply transformations to models
            rotateModel();

            // begin animations when viewport is finished loading
            viewport.Loaded += new RoutedEventHandler(animationStart);
            
            // add models to viewport
            modelGroup.Children.Add(handModel);
            modelGroup.Children.Add(thumbModel);
            modelVisual3D.Content = modelGroup;
            viewport.Children.Add(modelVisual3D);

            // set initial size of viewport
            viewport.Width = 50;
            viewport.Height = 50;
        }

        private void initializeScene()
        {
            // load models
            HandModel hm = new HandModel();
            handModel = hm.Model;
            thumbModel = hm.ThumbModel;

            // set up camera
            PerspectiveCamera camera = new PerspectiveCamera();

            // set camera field of view
            camera.FieldOfView = 60;
            
            // position camera to look down on hand from above
            camera.Position = new Point3D(0, 18, 0);
            camera.LookDirection = new Vector3D(0, -18, 0);
            camera.UpDirection = new Vector3D(0, 0, -1);

            // asign camera to viewport
            viewport.Camera = camera;

            // setup light
            DirectionalLight light = new DirectionalLight();
            light.Color = Colors.White;
            light.Direction = new Vector3D(0, -1, 0);
            modelGroup.Children.Add(light);

            // set handModel's transformations
            handModel.Transform = modelRotation;

            // setup thumbModel's transformations
            Transform3DGroup thumbTransform = new Transform3DGroup();
            
            TranslateTransform3D toOrigin = new TranslateTransform3D(new Point3D(0, 0, 0) - thumbBase);
            thumbTransform.Children.Add(toOrigin);

            thumbTransform.Children.Add(thumbRotation);
            
            TranslateTransform3D fromOrigin = new TranslateTransform3D(thumbBase - new Point3D(0, 0, 0));
            thumbTransform.Children.Add(fromOrigin);

            thumbTransform.Children.Add(modelRotation);
            thumbModel.Transform = thumbTransform;
        }

        private void rotateModel()
        {
            // add an intitial animation of with an "empty" transformation to the beginning of the rotation sequence
            Storyboard initialStoryboard = new Storyboard();
            Rotation3DAnimation initialRotation = new Rotation3DAnimation(new AxisAngleRotation3D(new Vector3D(1, 0, 0), 0.0), animationTime);
            initialStoryboard.Children.Add(initialRotation);
            handRotationAnimations.Add(initialStoryboard);

            Storyboard.SetTargetName(initialRotation, "modelRotation");
            Storyboard.SetTargetProperty(initialRotation, new PropertyPath(RotateTransform3D.RotationProperty));

            // add hand rotation animations to rotation sequence 
            foreach (Vector3D vector in movementData.HandRotationSequence)
            {
                // convert axes from data axes to WPF axes
                Vector3D convertedVector = convertAxes(vector);

                // create pitch/yaw/roll from data
                Quaternion pitch = new Quaternion(new Vector3D(1, 0, 0), convertedVector.X);
                Quaternion yaw = new Quaternion(new Vector3D(0, 1, 0), convertedVector.Y);
                Quaternion roll = new Quaternion(new Vector3D(0, 0, 1), convertedVector.Z);
                
                // apply rotations in order: pitch THEN yaw THEN roll
                Quaternion qr = yaw * pitch * roll;
                
                // create animation from rotation
                Rotation3DAnimation rotation = new Rotation3DAnimation(new QuaternionRotation3D(qr), animationTime);

                Storyboard storyboard = new Storyboard();
                storyboard.Children.Add(rotation);
                handRotationAnimations.Add(storyboard);
                
                Storyboard.SetTargetName(rotation, "modelRotation");
                Storyboard.SetTargetProperty(rotation, new PropertyPath(RotateTransform3D.RotationProperty));
            }

            if (movementData.IsVectorProduct)
                rotateThumb_VectorProduct();
            else
                rotateThumb_SensorFusion();
            
        }
        
        private void rotateThumb_VectorProduct()
        {
            rotateOntoPlane(); // place thumb on plane

            // add an intitial animation of with an "empty" transformation to the beginning of the rotation sequence
            Storyboard initialStoryboardThumb = new Storyboard();
            Rotation3DAnimation initialRotationThumb = new Rotation3DAnimation(new AxisAngleRotation3D(new Vector3D(1, 0, 0), 0.0), animationTime);
            initialStoryboardThumb.Children.Add(initialRotationThumb);
            thumbRotationAnimations.Add(initialStoryboardThumb);

            Storyboard.SetTargetName(initialRotationThumb, "thumbRotation");
            Storyboard.SetTargetProperty(initialRotationThumb, new PropertyPath(RotateTransform3D.RotationProperty));



            // add thumb rotation and theta animations to rotation sequence 
            foreach (Vector3D angle in movementData.ThumbRotationSequence)
            {
                Vector3D planeNormal = Vector3D.CrossProduct(movementData.PlaneVector_1, movementData.PlaneVector_2);

                // create thumb rotations
                Quaternion qr = new Quaternion(planeNormal, angle.X);
                
                // create animation from rotation
                Rotation3DAnimation rotation = new Rotation3DAnimation(new QuaternionRotation3D(qr), animationTime);

                Storyboard storyboard = new Storyboard();
                storyboard.Children.Add(rotation);
                thumbRotationAnimations.Add(storyboard);

                Storyboard.SetTargetName(rotation, "thumbRotation");
                Storyboard.SetTargetProperty(rotation, new PropertyPath(RotateTransform3D.RotationProperty));
            }
        }

        private void rotateThumb_SensorFusion()
        {
            // add an intitial animation of with an "empty" transformation to the beginning of the rotation sequence
            Storyboard initialStoryboardThumb = new Storyboard();
            Rotation3DAnimation initialRotationThumb = new Rotation3DAnimation(new AxisAngleRotation3D(new Vector3D(1, 0, 0), 0.0), animationTime);
            initialStoryboardThumb.Children.Add(initialRotationThumb);
            thumbRotationAnimations.Add(initialStoryboardThumb);

            Storyboard.SetTargetName(initialRotationThumb, "thumbRotation");
            Storyboard.SetTargetProperty(initialRotationThumb, new PropertyPath(RotateTransform3D.RotationProperty));

            // add thumb rotation and theta animations to rotation sequence 
            for (int i = 0; i < movementData.ThumbRotationSequence.Count; i++)
            {
                // calculate angle to rotate thumb based on theta
                double thumbThetaVector = movementData.ThetaSequence[i];
                double thetaPrime = calculateThetaPrime(thumbThetaVector);
                Quaternion thetaRotation = new Quaternion(new Vector3D(0, 1, 0), thetaPrime);

                // create thumb pitch/yaw/roll rotations
                Vector3D thumbRotationVector = movementData.ThumbRotationSequence[i];
                Vector3D convertedVector = convertAxes(thumbRotationVector);
                Quaternion pitch = new Quaternion(new Vector3D(1, 0, 0), convertedVector.X);
                Quaternion yaw = new Quaternion(new Vector3D(0, 1, 0), convertedVector.Y);
                Quaternion roll = new Quaternion(new Vector3D(0, 0, 1), convertedVector.Z);

                // apply rotations in order: thetaRotation THEN yaw THEN pitch THEN roll
                Quaternion qr = thetaRotation * yaw * pitch * roll;

                // create animation from rotation
                Rotation3DAnimation rotation = new Rotation3DAnimation(new QuaternionRotation3D(qr), animationTime);

                Storyboard storyboard = new Storyboard();
                storyboard.Children.Add(rotation);
                thumbRotationAnimations.Add(storyboard);

                Storyboard.SetTargetName(rotation, "thumbRotation");
                Storyboard.SetTargetProperty(rotation, new PropertyPath(RotateTransform3D.RotationProperty));
            }
        }

        private void rotateOntoPlane()
        {
            // convert vectors to use WPF axes
            Vector3D plane_v1 = convertAxes(movementData.PlaneVector_1);
            Vector3D plane_v2 = convertAxes(movementData.PlaneVector_2);

            // set handBase and handTip to points on Plane
            handBase = movementData.PlanePoint;
            handTip = plane_v1 + handBase;

            // rotate thumb to perpendicular to plane
            double thetaPrime = calculateThetaPrime(90);
            Quaternion thetaRotation = new Quaternion(new Vector3D(0, 1, 0), thetaPrime);

            Vector3D length_1 = thumbBase - thumbTip;
            Vector3D length_2 = handBase - thumbBase;

            // solve right triangle to get angle to rotate
            double alpha = Math.Acos(length_2.Length / length_1.Length);
            alpha = alpha * (180 / Math.PI); // convert rad to deg
            alpha = 180 - alpha;

            Quaternion alphaRotation = new Quaternion(new Vector3D(1, 0, 0), alpha);
            Quaternion onPlane_q = thetaRotation * alphaRotation;
            QuaternionRotation3D onPlane_qr = new QuaternionRotation3D(onPlane_q);
            RotateTransform3D onPlane = new RotateTransform3D(onPlane_qr);

            // set thumb transformations
            Transform3DGroup thumbTransform = new Transform3DGroup();
            TranslateTransform3D toOrigin2 = new TranslateTransform3D(new Point3D(0, 0, 0) - thumbBase);
            thumbTransform.Children.Add(toOrigin2);

            thumbTransform.Children.Add(onPlane);
            thumbTransform.Children.Add(thumbRotation);

            TranslateTransform3D fromOrigin2 = new TranslateTransform3D(thumbBase - new Point3D(0, 0, 0));
            thumbTransform.Children.Add(fromOrigin2);
            thumbTransform.Children.Add(modelRotation);
            thumbModel.Transform = thumbTransform;
        }

        // convert from data axes to WPF axes
        private Vector3D convertAxes(Vector3D vector)
        {
            return new Vector3D(-vector.Y, -vector.Z, -vector.X);
        }

        // begin animations
        private void animationStart(object sender, RoutedEventArgs e)
        {
            handRotationAnimations[0].Begin(viewport, HandoffBehavior.SnapshotAndReplace, true);
            thumbRotationAnimations[0].Begin(viewport, HandoffBehavior.SnapshotAndReplace, true);
        }

        // begin the next animation in sequence
        public void startNextAnimation()
        {
            // skip to completion of current animation before starting next
            if (currentRotationIndex >= 0 && currentRotationIndex < handRotationAnimations.Count)
            {
                handRotationAnimations[currentRotationIndex].SkipToFill(viewport);
                thumbRotationAnimations[currentRotationIndex].SkipToFill(viewport);
            }
            
            // begin next animation if it exists
            if (currentRotationIndex < handRotationAnimations.Count - 1)
            {
                handRotationAnimations[++currentRotationIndex].Begin(viewport, HandoffBehavior.SnapshotAndReplace, true);
                thumbRotationAnimations[currentRotationIndex].Begin(viewport, HandoffBehavior.SnapshotAndReplace, true);
            }
        }

        // begin the next animation in sequence
        public void startPreviousAnimation()
        {
            // skip to completion of current animation before starting next
            if (currentRotationIndex >= 0 && currentRotationIndex < handRotationAnimations.Count)
            {
                handRotationAnimations[currentRotationIndex].SkipToFill(viewport);
                thumbRotationAnimations[currentRotationIndex].SkipToFill(viewport);
            }

            // begin previous animation if it exists
            if (currentRotationIndex > 0)
            {
                handRotationAnimations[--currentRotationIndex].Begin(viewport, HandoffBehavior.SnapshotAndReplace, true);
                thumbRotationAnimations[currentRotationIndex].Begin(viewport, HandoffBehavior.SnapshotAndReplace, true);
            }
        }
        
        // calculate angle to rotate thumb from angle theta
        private double calculateThetaPrime(double theta)
        {
            // get vectors representing lines between hand and thumb points
            Vector3D length_1 = handBase - thumbBase;
            Vector3D length_2 = thumbBase - thumbTip;
            Vector3D length_3 = handBase - handTip;
            Vector3D length_4 = thumbBase - handTip;

            // calculate theta_2 (angle opposite theta) using Law of Cosines
            double L1_L3 = Math.Acos((length_1.LengthSquared + length_3.LengthSquared - length_4.LengthSquared) / (2 * length_1.Length * length_3.Length));
            L1_L3 = L1_L3 * (180 / Math.PI); // convert to radians
            double theta_2 = L1_L3 - theta;

            // calculate angle x using Law of Sines
            double x = Math.Sin(theta_2 * (Math.PI / 180) / length_2.Length);
            x = x * length_1.Length;
            x = x - 2;
            x = Math.Asin(x);

            // 
            double alpha = ((Math.Sin(theta_2 * (Math.PI / 180)) / length_2.Length) * length_1.Length);
            if (alpha > 1)
            {
                alpha = Math.Ceiling(alpha) - alpha;
            }
            if (alpha < -1)
            {
                alpha = Math.Floor(alpha) - alpha;
            }


            alpha = Math.Asin(alpha);
            alpha = alpha * (180 / Math.PI);

            double beta = 180 - theta;

            double failure = Math.Acos((length_1.LengthSquared + length_3.LengthSquared - length_4.LengthSquared) / (2 * length_1.Length * length_3.Length));
            failure = failure - Math.Asin(1 / length_1.Length);
            failure = failure * (180 / Math.PI);

            return 180 - alpha - beta;
        }

        // Rotate when labels are updated
        public void rotationsFromLabels(Vector3D handRot, Vector3D thumbRot, double theta)
        {
            Vector3D convertedVectorHand = convertAxes(handRot);
            Vector3D convertedVectorThumb = convertAxes(thumbRot);

            // Hand
            // create pitch/yaw/roll from data
            Quaternion pitchHand = new Quaternion(new Vector3D(1, 0, 0), convertedVectorHand.X);
            Quaternion yawHand = new Quaternion(new Vector3D(0, 1, 0), convertedVectorHand.Y);
            Quaternion rollHand = new Quaternion(new Vector3D(0, 0, 1), convertedVectorHand.Z);

            // apply rotations in order: pitch THEN yaw THEN roll
            Quaternion qrHand = yawHand * pitchHand * rollHand;
            QuaternionRotation3D qrrHand = new QuaternionRotation3D(qrHand);
            RotateTransform3D handTransform = new RotateTransform3D(qrrHand);
            modelRotation = handTransform;

            // Thumb
            // calculate angle to rotate thumb based on theta
            double thetaPrime = calculateThetaPrime(theta);
            Quaternion thetaRotation = new Quaternion(new Vector3D(0, 1, 0), thetaPrime);

            // create thumb pitch/yaw/roll rotations
            Quaternion pitchThumb = new Quaternion(new Vector3D(1, 0, 0), convertedVectorThumb.X);
            Quaternion yawThumb = new Quaternion(new Vector3D(0, 1, 0), convertedVectorThumb.Y);
            Quaternion rollThumb = new Quaternion(new Vector3D(0, 0, 1), convertedVectorThumb.Z);

            // apply rotations in order: thetaRotation THEN yaw THEN pitch THEN roll
            Quaternion qrThumb = thetaRotation * yawThumb * pitchThumb * rollThumb;
            QuaternionRotation3D qrrThumb = new QuaternionRotation3D(qrThumb);
            RotateTransform3D thumbTransformqr = new RotateTransform3D(qrrThumb);
            thumbRotation = thumbTransformqr;

            // set handModel's transformations
            handModel.Transform = modelRotation;

            // setup thumbModel's transformations
            Transform3DGroup thumbTransform = new Transform3DGroup();

            TranslateTransform3D toOrigin = new TranslateTransform3D(new Point3D(0, 0, 0) - thumbBase);
            thumbTransform.Children.Add(toOrigin);

            thumbTransform.Children.Add(thumbRotation);

            TranslateTransform3D fromOrigin = new TranslateTransform3D(thumbBase - new Point3D(0, 0, 0));
            thumbTransform.Children.Add(fromOrigin);

            thumbTransform.Children.Add(modelRotation);
            thumbModel.Transform = thumbTransform;
        }
    }
}