using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Timers;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Windows.Threading;

namespace modsymwpf
{
    /// <summary>
    /// Logika interakcji dla klasy MainWindow
    /// </summary>
    public partial class MainWindow : Window
    {
        static DispatcherTimer timer;

        public MainWindow()
        {
            InitializeComponent();
           
            Chart_posx1.DataContext = MyValue_posx1;
            Chart_posy1.DataContext = MyValue_posy1;
            Chart_posx2.DataContext = MyValue_posx2;
            Chart_posy2.DataContext = MyValue_posy2;
            
        }
        
        public void Czas()
        {
            timer = new DispatcherTimer();
            
            timer.Interval = TimeSpan.FromMilliseconds(10);//20
            timer.Tick +=timer_Tick;
            timer.Start();
            
        }       

        private void timer_Tick(object sender, EventArgs e)
        {
            calculate();
        }        

        private void Button_Click(object sender, RoutedEventArgs e)
        {

            try
            {
                theta1 = Convert.ToDouble(textTheta1.Text);//radiany
                theta2 = Convert.ToDouble(textTheta2.Text);
                Velocity1 = Convert.ToDouble(textVelocity1.Text);
                Velocity2 = Convert.ToDouble(textVelocity2.Text);//10
                L1 = Convert.ToDouble(textL1.Text);//1
                L2 = Convert.ToDouble(textL2.Text);//3
                m1 = Convert.ToDouble(textM1.Text);//2
                m2 = Convert.ToDouble(textM2.Text);//1
                
                n = 0;

                time = 0;
                //calculate();
                Czas();
                ButtonStop.IsEnabled = true;
                ButtonStart.IsEnabled = false;
                MyValue_posx1.Clear();
                MyValue_posy1.Clear();
                MyValue_posx2.Clear();
                MyValue_posy2.Clear();
                scroll.ScrollToTop();

            }
            catch
            {
                MessageBox.Show("Not correct data!");
            }
        }

        private void Button_Click_1(object sender, RoutedEventArgs e)
        {
            timer.Stop();
            ButtonStop.IsEnabled = false;
            ButtonStart.IsEnabled = true;
        }



        //---------****

        double theta1;
        double theta2;
        double Velocity1;
        double Velocity2;
        double time;
        double dt = 0.1;
        double a;
        double b;

        double L1;
        double L2;
        double m1;
        double m2;
        double g = 9.81;

        int n=0;

        ObservableCollection<KeyValuePair<double, double>> MyValue_posx1 = new ObservableCollection<KeyValuePair<double, double>>();
        ObservableCollection<KeyValuePair<double, double>> MyValue_posy1 = new ObservableCollection<KeyValuePair<double, double>>();
        ObservableCollection<KeyValuePair<double, double>> MyValue_posx2 = new ObservableCollection<KeyValuePair<double, double>>();
        ObservableCollection<KeyValuePair<double, double>> MyValue_posy2 = new ObservableCollection<KeyValuePair<double, double>>();

        public static void accel(double[] x, double[] v, double L1, double L2, double m1, double m2, double g, out double a, out double b)
        {

            double dTheta = x[1] - x[0];

            double num1 = m2 * L1 * v[0] * v[0] * Math.Sin(dTheta) * Math.Cos(dTheta) + m2 * g * Math.Sin(x[1]) * Math.Cos(dTheta) +
                       m2 * L2 * v[1] * v[1] * Math.Sin(dTheta) - (m1 + m2) * g * Math.Sin(x[0]);
            double den1 = (m1 + m2) * L1 - m2 * L1 * Math.Cos(dTheta) * Math.Cos(dTheta);

            double num2 = -m2 * L2 * v[1] * v[1] * Math.Sin(dTheta) * Math.Cos(dTheta) + (m1 + m2) *
                           (g * Math.Sin(x[0]) * Math.Cos(dTheta) - L2 * v[0] * v[0] * Math.Sin(dTheta) - g * Math.Sin(x[1]));

            double den2 = (m1 + m2) * L2 - m2 * L2 * Math.Cos(dTheta) * Math.Cos(dTheta);

            a = num1 / den1;
            b = num2 / den2;


        }
        public double[] x1 = new double[3];
        public double[] x2 = new double[3];
        public double[] x3 = new double[3];
        public double[] x4 = new double[3];
        public double[] v1 = new double[3];
        public double[] v2 = new double[3];
        public double[] v3 = new double[3];
        public double[] v4 = new double[3];
        public double[] a1 = new double[3];
        public double[] a2 = new double[3];
        public double[] a3 = new double[3];
        public double[] a4 = new double[3];
        
        public void calculate()
        {

            
            {
                n++;
                textN.Content = n.ToString();
                time = time + dt;
                x1[0] = theta1;
                v1[0] = Velocity1;
                x1[1] = theta2;
                v1[1] = Velocity2;


                accel(x1, v1, L1, L2, m1, m2, g, out a, out b);
                a1[0] = a;
                a1[1] = b;
                ////////////////////////////////////////////////////
                x2[0] = x1[0] + 0.5 * v1[0] * dt;
                v2[0] = v1[0] + 0.5 * a1[0] * dt;
                x2[1] = x1[1] + 0.5 * v1[1] * dt;
                v2[1] = v1[1] + 0.5 * a1[1] * dt;

                accel(x2, v2, L1, L2, m1, m2, g, out a, out b);


                a2[0] = a;
                a2[1] = b;

                /////////////////////////////////////////

                x3[0] = x1[0] + 0.5 * v2[0] * dt;
                v3[0] = v1[0] + 0.5 * a2[0] * dt;
                x3[1] = x1[1] + 0.5 * v2[1] * dt;
                v3[1] = v1[1] + 0.5 * a2[1] * dt;

                accel(x3, v3, L1, L2, m1, m2, g, out a, out b);


                a3[0] = a;
                a3[1] = b;
                //////////////////////////////////////////////////////////
                x4[0] = x1[0] + v3[0] * dt;
                v4[0] = v1[0] + a3[0] * dt;
                x4[1] = x1[1] + v3[1] * dt;
                v4[1] = v1[1] + a3[1] * dt;

                accel(x4, v4, L1, L2, m1, m2, g, out a, out b);
                a4[0] = a;
                a4[1] = b;
                ///////////////////////////////////////////////////////

                theta1 = x1[0] + (dt / 6.0) * (v1[0] + 2 * v2[0] + 2 * v3[0] + v4[0]);
                Velocity1 = v1[0] + (dt / 6.0) * (a1[0] + 2 * a2[0] + 2 * a3[0] + a4[0]);

                theta2 = x1[1] + (dt / 6.0) * (v1[1] + 2 * v2[1] + 2 * v3[1] + v4[1]);
                Velocity2 = v1[1] + (dt / 6.0) * (a1[1] + 2 * a2[1] + 2 * a3[1] + a4[1]);

                //var posx1 = L1 * Math.Sin(theta1);
                //var posy1 = -L1 * Math.Cos(theta1);//-L1
                //var posx2 = posx1 + L2 * Math.Sin(theta2);
                //var posy2 = posy1 - L2 * Math.Cos(theta2);//-L2




                var posx1 = L1 * Math.Sin(theta1);
                var posy1 = -L1 * Math.Cos(theta1);//-L1
                var posx2 = posx1 + L2 * Math.Sin(theta2);
                var posy2 = posy1 - L2 * Math.Cos(theta2);//-L2

               posy1 = posy1 * -1;
               posy2 = posy2 * -1;
                
          
                //nitka.X2 = 100 + (int)posx1;
                //nitka.Y2 = 100 + Math.Abs((int)posy1);
                //Point p = new Point(100 + posx1, 105 + Math.Abs((int)posy1));
                //ball.Center = p;
                //nitka2.X1 = 100 + posx1;
                //nitka2.Y1 = 105 + Math.Abs((int)posy1);
                //Point p2 = new Point(100 + posx2, 165 + Math.Abs((int)posy2));
                //ball2.Center = p2;
                //nitka2.X2 = 100 + posx2;
                //nitka2.Y2 = 165 + Math.Abs((int)posy2);


                nitka.X2 = 100 + posx1;
                nitka.Y2 = 80 + posy1;
                Point p = new Point(100 + posx1, 80 + posy1);
                ball.Center = p;
                nitka2.X1 = 100 + posx1;
                nitka2.Y1 = 80 + posy1;
                Point p2 = new Point(100 + posx2, 80 + posy2);
                ball2.Center = p2;
                nitka2.X2 = 100 + posx2;
                nitka2.Y2 = 80 + posy2;

                posy1 = posy1 * -1;
                posy2 = posy2 * -1;

                //nitka.X2 =posx1;
                //nitka.Y2 = posy1;
                //Point p = new Point(posx1, posy1);
                //ball.Center = p;
                //nitka2.X1 = posx1;
                //nitka2.Y1 = posy1;
                //Point p2 = new Point( posx2, posy2);
                //ball2.Center = p2;
                //nitka2.X2 = posx2;
                //nitka2.Y2 = posy2;
                
                

                MyValue_posx1.Add(new KeyValuePair<double, double>(time, posx1));
                MyValue_posy1.Add(new KeyValuePair<double, double>(time, posy1));
                MyValue_posx2.Add(new KeyValuePair<double, double>(time, posx2));
                MyValue_posy2.Add(new KeyValuePair<double, double>(time, posy2));


                
            } 

            

        }

        
    }
}
