using System;
using System.Windows;
using OpenTK;
using OpenTK.Graphics;
using OpenTK.Graphics.OpenGL;
using MathNet.Numerics.LinearAlgebra;

namespace NDof3DManipulatorFK
{
    /// <summary>
    /// MainWindow.xaml の相互作用ロジック
    /// </summary>
    public partial class MainWindow : Window
    {

        # region Camera__Field
        bool isCameraRotating;
        Vector2 current, previous;
        Matrix4 rotate;
        float zoom;
        //float wheelPrevious;
        # endregion

        # region Light__Field
        Vector4 lightPosition;
        Color4 lightAmbient;
        Color4 lightDiffuse;
        Color4 lightSpecular;

        Color4 materialAmbient;
        Color4 materialDiffuse;
        Color4 materialSpecular;
        float materialShininess;

        # endregion

        //glControlの起動時に実行される。
        private void glControl_Load(object sender, EventArgs e)
        {
            GL.ClearColor(Color4.Black);
            GL.Enable(EnableCap.DepthTest);


            # region Camera_Initialize
            this.isCameraRotating = false;
            this.current = Vector2.Zero;
            this.previous = Vector2.Zero;
            this.rotate = Matrix4.Identity;
            this.zoom = 1.0f;
            //this.wheelPrevious = 0.0f;
            # endregion

            # region Light_Initialize
            lightPosition = new Vector4(200.0f, 150f, 500.0f, 0.0f);
            lightAmbient = new Color4(0.2f, 0.2f, 0.2f, 1.0f);
            lightDiffuse = new Color4(0.7f, 0.7f, 0.7f, 1.0f);
            lightSpecular = new Color4(1.0f, 1.0f, 1.0f, 1.0f);

            materialAmbient = new Color4(0.24725f, 0.1995f, 0.0225f, 1.0f);
            materialDiffuse = new Color4(0.75164f, 0.60648f, 0.22648f, 1.0f);
            materialSpecular = new Color4(0.628281f, 0.555802f, 0.366065f, 1.0f);
            materialShininess = 51.4f;

            //裏面削除、反時計回りが表でカリング
            GL.Enable(EnableCap.CullFace);
            GL.CullFace(CullFaceMode.Back);
            GL.FrontFace(FrontFaceDirection.Ccw);

            //ライティングON Light0を有効化
            //GL.Enable(EnableCap.Lighting);
            //GL.Enable(EnableCap.Light0);

            //法線の正規化
            GL.Enable(EnableCap.Normalize);

            # endregion

        }

        //glControlのサイズ変更時に実行される。
        private void glControl_Resize(object sender, EventArgs e)
        {
            GL.Viewport(0, 0, glControl.Size.Width, glControl.Size.Height);
            GL.MatrixMode(MatrixMode.Projection);
            Matrix4 projection = Matrix4.CreatePerspectiveFieldOfView((float)Math.PI / 4, (float)glControl.Size.Width / (float)glControl.Size.Height, 1.0f, 64.0f);
            GL.LoadMatrix(ref projection);
        }

        //glControlの描画時に実行される。
        private void glControl_Paint(object sender, System.Windows.Forms.PaintEventArgs e)
        {
            glControl_Update();
        }

        // glControlの再描画のハブ
        private void glControl_Update()
        {
            glControl_draw();
        }
        public Vector<double> Cross(Vector<double> vector1, Vector<double> vector2)
        {
            //外積
            return Vector<double>.Build.DenseOfArray(new double[]{
                vector1[1] * vector2[2] - vector1[2]*vector2[1],
                vector1[2] * vector2[0] - vector1[0]*vector2[2],
                vector1[0] * vector2[1] - vector1[1]*vector2[0]});
        }
        //glControlの描画時に実行される。
        private void glControl_draw()
        {

            GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);

            #region TransFormationMatrix

            Matrix4 modelView = Matrix4.LookAt(Vector3.UnitZ * 10 / zoom, Vector3.Zero, Vector3.UnitY);
            GL.MatrixMode(MatrixMode.Modelview);
            GL.LoadMatrix(ref modelView);
            GL.MultMatrix(ref rotate);

            Matrix4 projection = Matrix4.CreatePerspectiveFieldOfView(MathHelper.PiOver4 / zoom, (float)this.Width / (float)this.Height, 1.0f, 64.0f);
            GL.MatrixMode(MatrixMode.Projection);
            GL.LoadMatrix(ref projection);

            #endregion

            # region Lighting
            GL.Light(LightName.Light0, LightParameter.Position, lightPosition);
            GL.Light(LightName.Light0, LightParameter.Ambient, lightAmbient);
            GL.Light(LightName.Light0, LightParameter.Diffuse, lightDiffuse);
            GL.Light(LightName.Light0, LightParameter.Specular, lightSpecular);

            GL.Material(MaterialFace.Front, MaterialParameter.Ambient, materialAmbient);
            GL.Material(MaterialFace.Front, MaterialParameter.Diffuse, materialDiffuse);
            GL.Material(MaterialFace.Front, MaterialParameter.Specular, materialSpecular);
            GL.Material(MaterialFace.Front, MaterialParameter.Shininess, materialShininess);
            #endregion

            #region AXES_PLOT
            GL.Begin(BeginMode.LineLoop);
            GL.LineWidth(3.0f);
            GL.Color4(Color4.White);
            GL.Vertex3(0, 0, 0);
            GL.Vertex3(0, 1, 0);
            GL.Color4(Color4.White);
            GL.Vertex3(0, 0, 0);
            GL.Vertex3(0, 0, 1);
            GL.Color4(Color4.White);
            GL.Vertex3(0, 0, 0);
            GL.Vertex3(1, 0, 0);
            GL.End();
            #endregion

            #region Robot_Arm
            GL.Begin(BeginMode.Lines);
            GL.LineWidth(13.0f);
            GL.Color4(Color4.Red);

            Color4[] colors = new Color4[]{
                Color4.Red, Color4.Blue,Color4.Green,
                Color4.Red, Color4.Blue,Color4.Green,
                Color4.Red, Color4.Blue,Color4.Green
            };

            var Vn = Vector<double>.Build.DenseOfArray(new double[3] { 1, 0, 0 });
            var ex = Vector<double>.Build.DenseOfArray(new double[3] { 1, 0, 0 });

            var V1 = Vector<double>.Build.DenseOfArray(new double[3] { 1, 1, 1 });
            var V2 = Vector<double>.Build.DenseOfArray(new double[3] { 1, 1, 1 });
            var V12 = Vector<double>.Build.DenseOfArray(new double[3] { 1, 1, 1 });

            var Vp = Vector<double>.Build.DenseOfArray(new double[3] { 1, 1, 1 });

            for (int i = 1; i < this.Pi.GetLength(0); i++)
            {
                GL.Color4(colors[i]);
                GL.Vertex3(this.Pi[i - 1, 0], this.Pi[i - 1, 1], this.Pi[i - 1, 2]);
                GL.Vertex3(this.Pi[i, 0], this.Pi[i, 1], this.Pi[i, 2]);
            }

            for (int i = 1; i < this.Pi.GetLength(0); i++)
            {
                GL.Color4(colors[i]);

                V1 = Vector<double>.Build.DenseOfArray(new double[3] { this.Pi[i - 1, 0], this.Pi[i - 1, 1], this.Pi[i - 1, 2] });
                V2 = Vector<double>.Build.DenseOfArray(new double[3] { this.Pi[i, 0], this.Pi[i, 1], this.Pi[i, 2] });
                // 単位ベクトル*0.2
                V12 = 0.2 * (V2 - V1);

                // 単位法線ベクトル
                Vn = Cross(V12, ex);
                Vn = Vn / Vn.L2Norm() * V12.L2Norm();
                // ターゲット
                Vp = V1 + V12 + Vn;
                // 法線ベクトル
                GL.Vertex3((V12 + V1).ToArray());
                GL.Vertex3(Vp.ToArray());
                // 斜めベクトル
                GL.Vertex3((V1).ToArray());
                GL.Vertex3(Vp.ToArray());
                GL.Vertex3((V2).ToArray());
                GL.Vertex3(Vp.ToArray());
            }

            GL.End();
            #endregion

            #region Robot_TargetPoint
            GL.Begin(BeginMode.Lines);
            GL.LineWidth(13.0f);
            GL.Color4(Color4.Gray);

            GL.Vertex3(this.Pi[0, 0], this.Pi[0, 1], this.Pi[0, 2]);
            GL.Vertex3(this.Pg[0], this.Pg[1], this.Pg[2]);

            GL.End();
            # endregion

            # region Draw_Grid
            int J = 5;
            GL.Begin(BeginMode.Lines);
            GL.Color4(0.3, 0.3, 0.3, 0.3);
            GL.LineWidth(0.1f);

            for (int i = -J; i <= J; i++)
            {
                GL.Vertex3(-J, i, 0);
                GL.Vertex3(J, i, 0);
                GL.Vertex3(i, -J, 0);
                GL.Vertex3(i, J, 0);
            }
            GL.End();
            # endregion

            glControl.SwapBuffers();
        }



        /// <summary>
        /// マウスイベント関連
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void glControl_MouseDown(object sender, System.Windows.Forms.MouseEventArgs e)
        {

            if (e.Button == System.Windows.Forms.MouseButtons.Right)
            {
                this.isCameraRotating = true;
                this.current = new Vector2(e.X, e.Y);

            }
            // 左マウスクリック
            if (e.Button == System.Windows.Forms.MouseButtons.Left)
            {
            }
        }

        private void glControl_MouseUp(object sender, System.Windows.Forms.MouseEventArgs e)
        {
            if (e.Button == System.Windows.Forms.MouseButtons.Right)
            {
                this.isCameraRotating = false;
                this.previous = Vector2.Zero;
            }

        }

        private void glControl_MouseMove(object sender, System.Windows.Forms.MouseEventArgs e)
        {
            if (this.isCameraRotating)
            {
                this.previous = this.current;
                this.current = new Vector2(e.X, e.Y);
                Vector2 delta = current - previous; // 移動方向ベクトル
                delta /= (float)Math.Sqrt(this.Width * this.Width + this.Height * this.Height); //対角線長さ
                float length = delta.Length;
                if (length > 0.0)
                {
                    float rad = length * MathHelper.Pi;
                    float theta = (float)Math.Sin(rad) / length;
                    Quaternion after = new Quaternion(delta.Y * theta, delta.X * theta, 0.0f, (float)Math.Cos(rad));
                    rotate = rotate * Matrix4.Rotate(after);
                }
                glControl_Update();
            }

        } // glControl_MouseUP

        private void glControl_MouseWheel(object sender, System.Windows.Forms.MouseEventArgs e)
        {
            float delta = (float)e.Delta;

            zoom *= (float)Math.Pow(1.1, delta / 120.0);

            if (zoom > 2.0f) zoom = 2.0f;
            if (zoom < 0.5f) zoom = 0.5f;

            glControl_Update();

        }

    }
}