using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.Windows.Forms;

namespace WindowsFormsApplication1
{
    class MistINS
    {
        private const double g = 9.8;

        private const int R = 6378100; //m

        public MistINS(double ax, double b, double Q, double T)
        {
            this.ax = ax;
            this.b = b;
            this.Q = Math.Sqrt(Q);
            this.T = T;
            Xk1 = new Matrix(3, 1);
            Xk = new Matrix(3, 1);
            F = new Matrix(3, 3);
            Noise = new Random(DateTime.Now.Millisecond);
            G = new Matrix(3, 1);
            Init();
        }
        
        public MistINS(Matrix F, double Q, int n, double x0)
        {
            this.Q = Math.Sqrt(Q);
            Xk1 = new Matrix(n, 1);
            Xk1.Mbody[0, 0] = x0;
            Xk = new Matrix(n, 1);
            Xk.Mbody[0, 0] = x0;
            this.F = new Matrix(n, n);
            this.F = F;
            Noise = new Random(DateTime.Now.Millisecond);
            G = new Matrix(n, 1);
        }
        

        public void Init ()
        {
            CalcF();
            CalcG();
            SetX0();
        }

        Matrix Xk;

        public Matrix Xk1 { get; private set; }

        public Matrix F { get; set; }

        Matrix G;

        private double ax;
        public double Bias_x 
        { 
            get { return ax; }
            set { ax = value; }
        }

        private double b;
        public double Betta 
        {
            get { return b; }
            set { b = value; }
        }

        private double ey;
        public double Drift_y
        { 
            get { return ey; }
            set { ey = value; }
        }

        private double T;
        public double Period 
        {
            get { return T; }
            set { T = value; }
        }

        private Random Noise;
        public double Q { get; set; }

        public double w()
        {
            return Noise.Next(-100, 100)*1e-2*Q;
        }

        public void SaveToFile(string Name)
        {
            string[] lines = { ax.ToString("e"), b.ToString("e"), Q.ToString("e"), T.ToString("e") };
            File.WriteAllLines(Name, lines);  
        }

        public void LoadFromFile(string Name)
        {
            string[] lines = File.ReadAllLines(Name);
            ax = double.Parse(lines[0]);
            b = double.Parse(lines[1]);
            Q = double.Parse(lines[2]);
            T = double.Parse(lines[3]);
        }

        private void SetX0()
        {
            Xk.Mbody[0, 0] = 0;
            Xk.Mbody[1, 0] = -ax;
            Xk.Mbody[2, 0] = 0;
            Xk1 = Xk;
        }

        private void CalcF()
        {
            F.Mbody[0, 0] = 1;
            F.Mbody[0, 1] = -g * T;
            F.Mbody[0, 2] = 0;
            F.Mbody[1, 0] = T / R;
            F.Mbody[1, 1] = 1;
            F.Mbody[1, 2] = T;
            F.Mbody[2, 0] = 0;
            F.Mbody[2, 1] = 0;
            F.Mbody[2, 2] = 1 - b * T;
        }

        private void CalcG()
        {
            G.Mbody[0, 0] = 0;
            G.Mbody[1, 0] = 0;
            G.Mbody[2, 0] = T;
        }

        private double wk;
        private Matrix temp = new Matrix(3, 1);

        public void  Next()
        {
            wk = w();
         //   MessageBox.Show(wk.ToString("e"));
            Xk1 = (F * Xk) + (G * wk);
          //  MessageBox.Show(Xk.Mbody[2,0].ToString("e"));
            Xk.Mbody = Xk1.Mbody;
        }
    }
}