using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using CSML;

namespace PG_Sim
{
    static class Physics
    {
        // start time
        public static DateTime startTime;
        public static DateTime prevTime;
        public static bool run_solver=false;

        #region Constants
        const double g = 9.80556; // [m.s-2]
        const double startHeight = 50; // [m]
        const double m_pilot = 70;       // hmotnost pilota [kg]
        const double m_sedacka = 5;      // hmotnost sedacky [kg]
        const double m_padak = 5;        // hmotnost padaku [kg]
        const double m_vzduch = 1.2*5;   // hmotnost vzduchu v padaku [kg] - hustota * odhadnuty objem
        const double h_celk = 8;         // vyska od teziste pilota po teziste padaku [m]
        const double h_pilot = 1.8;      // vyska pilota [m]
        const double s_padak = 9.4;      // sirka padaku [m]
        const double M = m_pilot + m_sedacka + m_padak; // celkova hmotnost
        #endregion

        #region tensor of inertia
        // y = kloneni, x = klopeni, z = zataceni
        static double Ix = (0.0833 * m_pilot * h_pilot * h_pilot + (m_padak + m_vzduch) * h_celk * h_celk);
        static double Iy = (0.0833 * m_pilot * Math.Pow(h_pilot * 0.7, 2) + (m_padak + m_vzduch) * h_celk * h_celk);
        static double Iz = (0.0833 * m_pilot * Math.Pow((h_pilot * 0.7), 2) + 0.04167 * (m_padak + m_vzduch) * s_padak * s_padak);

        // complete inertia tensor
        static Matrix I = new Matrix(new double[,] { { Ix, 0, 0 }, { 0, Iy, 0 }, { 0, 0, Iz } });//new Matrix(new double[,] { { Ix, 20, 30 }, { 15, Iy, 22 }, { 14, 35, Iz } });//[722.89244 20 30;15 713.2572956 22;14 35 49.7588688]
        static Matrix invI = I.Inverse();
        #endregion

        // helpful variables
        static Matrix osZ = new Matrix(new double[,] { { 0 }, { 0 }, { 1 } });
        static double alpha=0,gama=0;

        #region state variables 
        private static Matrix position, velocity;        // position, velocity in three axes [m,m/s]
        private static Matrix velocityE;                 // velocity in global position system [m/s]   
        private static Matrix angPosition, omega;        // angular position, velocity in three axes [rad,rad/s]
        private static Matrix DCM;                       // direct cosine matrix   
        #endregion 

        #region forces lists (because of rendering)
        private static Matrix force,torque;             // in three axes
        public static List<Force> Forces = new List<Force> { };
        public static List<Force> FPrint = new List<Force> { };
        public static List<Force> aeroForces = new List<Force> { };
        #endregion

        #region controls
        private static double leftBrake = 0, rightBrake = 0, speed = 0, harness=0;
        #endregion

        // orientace padáku při startu
        private static Matrix osP = new Matrix(new double[,] { { 0 }, { 1 }, { 0 } });

        // působiště jednotlivých sil
        private static Matrix center = new Matrix(new double[,] { { 0 }, { 0 }, { h_celk } });
        private static Matrix pilotPos = new Matrix(new double[,] { { 0 }, { 0 }, { -7.5 } });

        // sets a default values
        public static void Init() 
        {
            position = new Matrix(new double[,] { { 0 }, { 0 }, { 500 } });
            velocity = new Matrix(new double[,] { { 0 }, { 10 }, { -1 } });
            velocityE = new Matrix(new double[,] { { 0 }, { 0 }, { 0 } });

            angPosition = new Matrix(new double[,] { { 0 }, { 0 }, { 0 } });
            omega = new Matrix(new double[,] { { 0 }, { 0 }, { 0 } });

            force = new Matrix(new double[,] { { 0 }, { 0 }, { 0 } });
            torque = new Matrix(new double[,] { { 0 }, { 0 }, { 0 } });

            DCM = new Matrix(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } });

            Import handle = new Import("force2_pusobiste.txt");
            double[][] startForce = handle.loadPoints();

            handle = new Import("force2_smer.txt");
            double[][] directForce = handle.loadPoints();

            for (int i =0;i<Support.settings.numForces;i++)
            {
                Matrix actionPoint = new Matrix(new double[,] { { startForce[i][0] }, { startForce[i][1] }, { startForce[i][2] } });
                Matrix dirPoint = new Matrix(new double[,] { { directForce[i][0] }, { directForce[i][1] }, { directForce[i][2] } });
                Matrix direction = dirPoint - actionPoint;

                aeroForces.Add(new Force(direction, actionPoint, "aero_lift"));
            }
            
        }

        /// <summary>
        /// Return actual position (distance and rotation) of glider
        /// </summary>
        public static kinematics GetPosition() 
        {
            return new kinematics(position, angPosition,omega, velocityE, torque, leftBrake, rightBrake, speed, harness,alpha,gama);
        }

        // integrating method
        public static void Solver()
        {
            // before start reset time
            startTime = DateTime.Now;
            prevTime = DateTime.Now;
            Console.WriteLine("Solver start!");

            // seems like infinite loop (but it is not)
            while (run_solver)
            {
                // first we get a fresh forces
                aerodynamics3D();

                // than make a few steps in solution
                for (int i = 0; i < 5; i++)
                {
                    // get integration step
                    TimeSpan delta_t = (DateTime.Now - prevTime);
                    prevTime = DateTime.Now;

                    // step is too small?
                    if (delta_t.TotalMilliseconds == 0)
                        // wait a while
                        System.Threading.Thread.Sleep(1);
                    else
                        // solve one step in solution
                        Six_DOF(delta_t);
                }
            }
            Console.WriteLine("Solver exit!");
        }
        
        private static void aerodynamics3D()
        {
            /*
             * Výpočet aerodynamiky se silami po délce padáku, odporem pilota
             * a pohybem v jedné rovině. Úhel alpha je definovaný jako úhel
             * náběhu křídla vzhledem k výslednici rychlosti.
             * 
             */
            // vyprázdnění balíku čar k vykreslení
            FPrint = new List<Force> { };
            force = Matrix.Zeros(3, 1);
            Matrix forceL = Matrix.Zeros(3, 1);
            Matrix forceR = Matrix.Zeros(3, 1);
            torque = Matrix.Zeros(3, 1);

            // projdeme všechny síly v seznamu a spočteme L,D
            for (int i = 0; i < aeroForces.Count; i++)
            {
                // složka rychlosti ve středu padáku od úhlové rychlosti
                Matrix v_omega = Matrix.Cross(omega, aeroForces[i].A);

                // velikost rychlosti v lokálních souřadnicích pro místo středu padáku
                Matrix v_celk = velocity + v_omega;
                double v = v_celk.Size();

                // výpočet delta
                double delta_i = delta(i);

                // pro výpočet vztlaku uvažujeme jen rychlost bez x složky (zahrneme jenom do odporu)
                Matrix velocity_lift = new Matrix(new double[,] { { 0 }, { v_celk[2,1].Re }, { v_celk[3, 1].Re } });

                // zjistíme úhel mezi rychlostí a osou padáku pro výpočet vztlaku
                double gama_lift = Matrix.getAngle(velocity_lift, osP);

                // gama pro odpor
                double gama_drag = Matrix.getAngle(velocity, osP);

                // spočteme L a D
                double L = (0.026) * harnessEffect(i + 1) * mpy(i + 1) * cL(delta_i + gama_lift) * Math.Pow(v, 2);
                double D = (0.021) * mpy(i + 1) * cD(delta_i + gama_drag) * Math.Pow(v, 2);
                
                // odporovou sílu natočíme proti směru proudu vzduchu
                Matrix vF = rotateForce(aeroForces[i].u, v_celk, L, D);

                // kvůli debugu rozdělíme na dvě L a D
                Matrix lift = new Matrix(new double[,] { { vF[1, 1].Re }, { vF[2, 1].Re }, { vF[3, 1].Re } });
                Matrix drag = new Matrix(new double[,] { { vF[1, 2].Re }, { vF[2, 2].Re }, { vF[3, 2].Re } }); 
                if (i < 27)
                    forceL += lift+drag;
                else
                    forceR += lift+drag;

                FPrint.Add(new Force(lift, aeroForces[i].A, "lift", lift.Size() * 5));
                FPrint.Add(new Force(drag, aeroForces[i].A, "drag", drag.Size() * 5));
                
                // spočteme moment
                try
                {
                    torque += Matrix.Cross(aeroForces[i].A, lift+drag);
                }
                catch
                { }
            }
            force = forceL + forceR;

            // tlumení momentu
            Matrix Btl = new Matrix(new double[,] { { -100 }, { -250}, { -250 } });
            torque += Matrix.DotProd(omega, Btl); 

            FPrint.Add(new Force(osP, Matrix.Zeros(3, 1), "tmodra", osP.Size() * 1000));
            FPrint.Add(new Force(velocity, Matrix.Zeros(3,1), "drag", velocity.Size()*100));

            // dále přičteme gravitační sílu
            Matrix Fg = DCM * new Matrix(new double[,] { { 0 }, { 0 }, { -M * g } });
            force += Fg;
            
            // a odporovou sílu pilota v sedačce
            Matrix Fo = -0.21 * Matrix.SignSquare(velocity);
            force += Fo;
        }

        private static double harnessEffect(int chamber)
        {
            // spočítá vliv náklonu sedačky
            return 1 + (chamber - 27) * (harness / (10*27));
        }

        private static void countForces2() 
        {
            // zjistíme dopřednou rychlost jako v_d = sqrt( vx^2 + vy^2 )  - ošetřit znamínko
            double vd = Math.Sqrt(Math.Pow(velocity[1, 1].Re, 2) + Math.Pow(velocity[2, 1].Re, 2));
            // a opadání
            double vo = velocity[3, 1].Re;
            
            // celková rychlost
            double vc = velocity.Size();

            // úhel klouzání
            double gama = Math.Atan2(vo, vd);

            // spočítáme rotační matici kolem podélné vodorovné a svislé osy
            double rx = angPosition[1, 1].Re;
            double ry = angPosition[2, 1].Re;
            double rz = angPosition[3, 1].Re;
            Matrix Rx = new Matrix(new double[,] { { 1, 0, 0 }, { 0, Math.Cos(rx), -Math.Sin(rx) }, { 0, Math.Sin(rx), Math.Cos(rx) } });
            Matrix Ry = new Matrix(new double[,] { { Math.Cos(ry), 0, Math.Sin(ry) }, { 0, 1, 0 }, { -Math.Sin(ry), 0, Math.Cos(ry) } });
            Matrix Rz = new Matrix(new double[,] { { Math.Cos(rz), -Math.Sin(rz), 0 }, { Math.Sin(rz), Math.Cos(rz), 0 }, { 0, 0, 1 } });
            Matrix R = Rx * Ry * Rz;
            /*Matrix R = DCM.Transpose();*/

            // projdeme všechny komory a spočteme L a D
            int i = 1; force = Matrix.Zeros(3, 1); torque = Matrix.Zeros(3, 1);
            FPrint = new List<Force> { };
            for (int j = 0; j < Forces.Count; j++)
            {
                if (Forces[j].type == "aero_lift")
                {
                    // lokální rychlost
                    Matrix vlm = DCM.Transpose() * velocity;// +Matrix.Cross(omega, Forces[j].A);
                    double vl = vlm.Size();

                    // spočteme velikost aerodynamických sil L a D
                    double L = mpy(i) * cL(alpha) * Math.Pow(vl, 2);
                    double D = mpy(i) * cD(alpha) * Math.Pow(vl, 2);

                    // natočíme je do směru letu
                    Matrix Fa = Matrix.Zeros(3, 1);//rotateForce(Forces[j].u, gama, L, D);

                    // uložíme pro tisk
                    FPrint.Add(new Force(R * Fa, R * Forces[j].A, "lift", Fa.Size()));

                    // zahrneme do sumy sil
                    force += Ry * Rz * Fa;

                    // spočítáme moment
                    torque += Matrix.Cross(Forces[j].A, Fa);

                    // počítáme komory 
                    i++;
                }
            }

            // tlumení v kývání
            Matrix B = new Matrix(new double[,] { { -1500,0,0 }, { 0,-800,0 }, { 0,0,-400 } });
            Matrix Mb = B * omega;

            // spočtem gravitační sílu
            Matrix Fg = new Matrix(new double[,] { { 0 }, { 0 }, { -M * g } });

            // zjistíme pozici působiště gravitační síly
            Matrix G_pusobiste = R * pilotPos;

            // předáme k vytisknutí gravitační sílu
            FPrint.Add(new Force(Fg, G_pusobiste, "gravity",-M*g));
 
            //spočtem moment
            Matrix torqueG = Matrix.Cross(G_pusobiste, Fg);
            torque += torqueG + Mb;

            // zahrneme gravitaci do silové výslednice
            force += Fg;

            // vytiskneme si výslednici
            FPrint.Add(new Force(force, Matrix.Zeros(3, 1), "suma", force.Size()));
        }
        
        private static double mpy(int chamber) 
        {
            // kvůli symetrii rozdělíme na dva případy
            if (1 <= chamber && chamber <=27)
            {
                double ch4 = Math.Pow(chamber,4);
                double ch3 = Math.Pow(chamber,3);
                double ch2 = Math.Pow(chamber,2);
                double ch  = chamber;
                
                return -7.856e-007 * ch4 + 8.485e-005* ch3 - 0.003698 * ch2 + 0.07599* ch + 0.3998;
            }
            else if (28<= chamber && chamber <=54)
            {
                double ch = 55-chamber;
                double ch4 = Math.Pow(ch,4);
                double ch3 = Math.Pow(ch,3);
                double ch2 = Math.Pow(ch,2);

                return -7.856e-007 * ch4 + 8.485e-005 * ch3 -0.003698 * ch2 + 0.07599 * ch + 0.3998;
            }
            else
                return 0;
        }

        private static double delta(int i)
        {
            /*
             * Delta je definována jako úhel mezi tětivou profilu a vodorovnou rovinou
             * (tedy rovinou, jejíž normála je osa z). Je dán polohou ovládacích prvků.
             * 
             * Musíme však připočíst ještě přírůstek od klopení padáku
             * 
             */
            //return -speed * 8 + 10 + leftBrake * 25;
            //---------------------------------------

            const double ofset = 25; // posunutí středu
            const double centr_offset = 10;
            const double ss_offset = 8;
            const double usi = 20;
            const double spad = 3;
            // znormujeme speed system
            double ssystem = -speed * ss_offset;
            double leftbrake = leftBrake * ofset;
            double rightbrake = rightBrake * ofset;

            // zjistíme o kolik se liší stažení levé strany od pravé
            double dif = leftbrake - rightbrake;

            if (dif == 0) // pokud je stažení na obou stranách stejné
                return ssystem + centr_offset + leftbrake;
            else if (dif < 0) // pravá strana je stažena více
                return ssystem +centr_offset+ Math.Max(leftbrake, Math.Min(rightbrake, -(dif / spad) * (i) + (dif / spad) * (54 - usi - spad) + leftbrake));
            else if (dif > 0) // levá strana je víc stažena
                return ssystem + centr_offset + Math.Max(rightbrake, Math.Min(leftbrake, -(dif / spad) * (i) + (dif / spad) * (usi + spad) + rightbrake));
            else
                return 0;
           
        }

        private static double cL(double alpha)
        {
            double p1, p2, p3, p4, p5, p6, p7, a, b, c, d, x,cl=0;
            // ošetření přetečení a podtečení
            if (alpha < 0)
                x = (alpha + 360) % 360;
            else if (alpha > 360)
                x = alpha % 360;
            else
                x = alpha;

            // první část z rychlostní poláry
            if (x <= 12.1240)
            {
                p1 = -0.0419;
                p2 = 7.71;
                cl = p1 * x + p2;
            }
            else if (x <= 52.6127)
            {
                a = 6.365;
                b = 0.02089;
                c = -6057;
                d = -0.713;
                cl = a * Math.Exp(b * x) + c * Math.Exp(d * x);
            }
            else if (x > 52.6127)
            {
                p1 = 1.834e-012;
                p2 = -2.49e-009;
                p3 = 1.361e-006;
                p4 = -0.0003815;
                p5 = 0.05766;
                p6 = -4.441;
                p7 = 137.3;
                cl = p1 * Math.Pow(x, 6) + p2 * Math.Pow(x, 5) + p3 * Math.Pow(x, 4) + p4 * Math.Pow(x, 3) + p5 * Math.Pow(x, 2) + p6 * x + p7;
            }
            return cl;
        }

        private static double cD(double alpha)
        {
            double a0, a1, b1, a2, b2, a3, b3, a4, b4,w, x = 0;
            // ošetření přetečení a podtečení
            if (alpha < 0)
                x = (alpha + 360) % 360;
            else if (alpha > 360)
                x = alpha % 360;
            else
                x = alpha;
            // Coefficients:
            a0 = 5.341;
            a1 = -0.3207;
            b1 = 0.004287;
            a2 = -5.325;
            b2 = 0.006063;
            a3 = -0.3293;
            b3 = 0.004287;
            a4 = 1.334;
            b4 = 0.1271;
            w = 0.01745;
            
            // CD: General model Fourier4:
            return a0 + a1 * Math.Cos(x * w) + b1 * Math.Sin(x * w) + a2 * Math.Cos(2 * x * w) + b2 * Math.Sin(2 * x * w) + a3 * Math.Cos(3 * x * w) + b3 * Math.Sin(3 * x * w) + a4 * Math.Cos(4 * x * w) + b4 * Math.Sin(4 * x * w);
        }
 
        private static Matrix rotateForce(Matrix u, Matrix velocity, double lengthL, double lengthD)
        {
            // POZN: vektory uvažujeme jednotkové a až na závěr spravíme
            // směr odporu je vždy proti směru pohybu (rychlosti) padáku
            Matrix D = -velocity/velocity.Size();

            // zjistíme osu rotace
            Matrix w = Matrix.Cross(u, D);
            // znormujeme
            Matrix os = w / Math.Sqrt(Math.Pow(w[1, 1].Re, 2) + Math.Pow(w[2, 1].Re, 2) + Math.Pow(w[3, 1].Re, 2));

            // pro zpřehlednění vykucháme složky vektoru
            double os1 = os[1,1].Re;
            double os2 = os[2,1].Re;
            double os3 = os[3,1].Re;

            // otočíme podle této osy o -90°
            Matrix R = new Matrix(new double[,] { { Math.Pow(os1, 2), os3 + os1 * os2, os1 * os3 - os2 }, { os1 * os2 - os3, Math.Pow(os2, 2), os1 + os2 * os3 }, { os2 + os1 * os3, os2 * os3 - os1, Math.Pow(os3, 2) } });
            Matrix L = R * D;

            // na závěr z jednotkových vektorů vyrobíme ty se správnou velikostí
            L *= lengthL;
            D *= lengthD;      

            // vrátíme promítnuté souřadnice
            return new Matrix(new double[,] { { L[1, 1].Re, D[1, 1].Re, 0 }, { L[2, 1].Re, D[2, 1].Re, 0 }, { L[3, 1].Re, D[3, 1].Re, 0 } });
        }

        private static void Six_DOF(TimeSpan delta_T)
        {
            // TODO: check nozero time

            #region Angular Position
            Matrix angAcceleration = invI * (torque - Matrix.Cross(omega, I * omega));

            // angular velocity
            omega += angAcceleration * delta_T.TotalSeconds;
            double p = omega[1, 1].Re;
            double q = omega[2, 1].Re;
            double r = omega[3, 1].Re;

            // angular position from last step
            double phi = angPosition[1, 1].Re;
            double theta = angPosition[2, 1].Re;
            double psi = angPosition[3, 1].Re;

            // count derivative angular position in euler angles
            double dotphi = p + (q * Math.Sin(phi) + r * Math.Cos(phi)) * (Math.Sin(theta) / Math.Cos(theta));
            double dottheta = q * Math.Cos(phi) - r * Math.Sin(phi);
            double dotpsi = (q * Math.Sin(phi) + r * Math.Cos(phi)) / (Math.Cos(theta));

            // make vector of derivative ang.pos.
            Matrix rotdot = new Matrix(new double[,] { { dotphi }, { dottheta }, { dotpsi } });

            // integrates angular position in Euler angles
            angPosition += rotdot * delta_T.TotalSeconds;
            #endregion

            #region linear coordinates
            Matrix acceleration = Matrix.Zeros(3, 1);
            try
            {
                acceleration = force / M + Matrix.Cross(velocity, omega); // 2nd Newton law
            }
            catch
            {
            }

            // count velocity (first integration)
            velocity += acceleration * delta_T.TotalSeconds;

            // count direct cosinus matrix
            DCM = DirectCosMatrix(angPosition);

            // velocity in global position system
            velocityE = DCM.Transpose() * velocity;

            // count position (second integration)
            position += velocityE * delta_T.TotalSeconds;
            #endregion
        }

        // counts 3x3 matrix of direct cosinus
        private static Matrix DirectCosMatrix(Matrix angles)
        {
            double Rx = angles[1, 1].Re;
            double Ry = angles[2, 1].Re;
            double Rz = angles[3, 1].Re;

            double cx = Math.Cos(Rx);
            double cy = Math.Cos(Ry);
            double cz = Math.Cos(Rz);

            double sx = Math.Sin(Rx);
            double sy = Math.Sin(Ry);
            double sz = Math.Sin(Rz);

            return new Matrix(new double[,] { { cy * cz, cy * sz, -sy }, { sx * sy * cz - cx * sz, sx * sy * sz + cx * cz, sx * cy }, { cx * sy * cz + sx * sz, cx * sy * sz - sx * cz, cx * cy } });

        }

        public static void Controlls(string label)
        {
            const double inflce = 0.02;
            if (label == "left")
            {
                leftBrake = Math.Min(Math.Max(leftBrake + inflce, 0), 1);
                rightBrake = Math.Min(Math.Max(rightBrake - inflce, 0), 1);
                harness = Math.Min(Math.Max(harness - inflce, -1), 1);
            }

            if (label == "right")
            {
                rightBrake = Math.Min(Math.Max(rightBrake + inflce, 0), 1);
                leftBrake = Math.Min(Math.Max(leftBrake - inflce, 0), 1);
                harness = Math.Min(Math.Max(harness + inflce, -1), 1);
            }

            if (label == "bothUp")
            {
                leftBrake = Math.Min(Math.Max(leftBrake + inflce, 0), 1);
                rightBrake = Math.Min(Math.Max(rightBrake + inflce, 0), 1);
            }

            if (label == "bothDown")
            {
                leftBrake = Math.Min(Math.Max(leftBrake - inflce, 0), 1);
                rightBrake = Math.Min(Math.Max(rightBrake - inflce, 0), 1);
            }
                
        }
        public static void Controlls(Controls values)
        {
            const double max = 2;
            leftBrake = Math.Min(Math.Max(leftBrake + values.leftBrake, 0), max);
            rightBrake = Math.Min(Math.Max(rightBrake + values.rightBrake, 0), max);

            harness = Math.Min(Math.Max(harness + values.harness, -1), 1);
            speed = Math.Min(Math.Max(speed + values.speed, 0), 1);
        }
        public static void Controlls(double left, double right, double tilt, double ss)
        {
            leftBrake = left;
            rightBrake = right;
            harness = tilt;
            speed = ss;
        }
    }

    public struct Force 
    {
        // direction vector
        public Matrix u;
        // point of action
        public Matrix A;
        // size of force
        public double F;
        // type of force (gravity, aerodynamical,...)
        public string type;

        public Force(Matrix direction, Matrix actionPoint, string forceType)
        {
            u = direction;
            A = actionPoint;
            type = forceType;
            F = 0; // default value
        }
        public Force(Matrix direction, Matrix actionPoint, string forceType,double size)
        {
            u = direction;
            A = actionPoint;
            type = forceType;
            F = size;
        }

        /// <summary>
        /// Get a coordinates of force final point
        /// </summary>
        /// <returns>Coordinates in matrix</returns>
        public Matrix finalPoint()
        {
            double p;
            if (u[3, 1].Re == 0 && u[2, 1].Re == 0 && u[1, 1].Re == 0)
                p = 0;
            else
                p = Math.Sqrt(Math.Pow(F / 100, 2) / (Math.Pow(u[1, 1].Re, 2) + Math.Pow(u[2, 1].Re, 2) + Math.Pow(u[3, 1].Re, 2)));
            return ((p * u) + A);
        }

        public Matrix getMoment()
        {
            return Matrix.Cross(A, F * u);
        }
    }

    public struct kinematics 
    {
        // Position coordinates [m]
        public double x,y,z;
        // Velocity [m/s]
        public double vx, vy, vz;

        // Rotation coordinates [°]
        public double rx;    // around x axis
        public double ry;    // around y axis     
        public double rz;    // around z axis
        // Rotation velocity
        public double rvx,rvy,rvz;

        // torque
        public double Mx, My, Mz;

        // controls
        public double leftB, rightB, speed, harness,degAlpha,degGama;

        public kinematics(Matrix position, Matrix angle,Matrix omega,Matrix velocity, Matrix torque,double leftBrake,double rightBrake,double speedSystem, double harnessTilt,double alpha,double gama) 
        {
            // coords
            x = position[1, 1].Re;
            y = position[2, 1].Re;
            z = position[3, 1].Re;

            // angles
            rx = angle[1, 1].Re;
            ry = angle[2, 1].Re;
            rz = angle[3, 1].Re;

            // angles
            rvx = omega[1, 1].Re;
            rvy = omega[2, 1].Re;
            rvz = omega[3, 1].Re;

            // velocity
            vx = velocity[1, 1].Re;
            vy = velocity[2, 1].Re;
            vz = velocity[3, 1].Re;

            // torque
            Mx = torque[1, 1].Re;
            My = torque[2, 1].Re;
            Mz = torque[3, 1].Re;

            // Controls
            leftB = leftBrake;
            rightB = rightBrake;
            speed = speedSystem;
            harness = harnessTilt;
            degAlpha = alpha;
            degGama = gama;
        }
    }
}
