#region pre-script
using System;
using System.Text;
using System.Collections;
using System.Collections.Generic;
using VRageMath;
using VRage.Game;
using Sandbox.ModAPI.Interfaces;
using Sandbox.ModAPI.Ingame;
using Sandbox.Game.EntityComponents;
using VRage.Game.Components;
using VRage.Collections;
using VRage.Game.ObjectBuilders.Definitions;
using VRage.Game.ModAPI.Ingame;
using SpaceEngineers.Game.ModAPI.Ingame;

namespace IngameScript
{
public class Program : MyGridProgram
{
        #endregion

        static string reference_block = "Remote Control";

        // GAINS
        static double proportionalGain = 5.0;
        static double integralGain = 0.1;
        static double derivativeGain = 0.1;

        //OTHER PARAMETERS
        //static double integralLimit = 10;         //limit the integral will be clamped to
        static double timeStep = 0.01;             //time step between control inputs
        static double integralDecayFactor = 0.25; //Integral sum will reduce by this proportion each cycle

        //Whip's PID controller class v4 - 8/27/17
        public class PID
        {
            double _kP = 0;
            double _kI = 0;
            double _kD = 0;
            double _integralDecayRatio = 0;
            double _lowerBound = 0;
            double _upperBound = 0;
            double _timeStep = 0;
            double _errorSum = 0;
            double _lastError = 0;
            bool _firstRun = true;
            bool _integralDecay = false;
            public double Value { get; private set; }

            public PID(double kP, double kI, double kD, double lowerBound, double upperBound, double timeStep)
            {
                _kP = kP;
                _kI = kI;
                _kD = kD;
                _lowerBound = lowerBound;
                _upperBound = upperBound;
                _timeStep = timeStep;
                _integralDecay = false;
            }

            public PID(double kP, double kI, double kD, double integralDecayRatio, double timeStep)
            {
                _kP = kP;
                _kI = kI;
                _kD = kD;
                _timeStep = timeStep;
                _integralDecayRatio = integralDecayRatio;
                _integralDecay = true;
            }

            public double Control(double error)
            {
                //Compute derivative term
                var errorDerivative = (error - _lastError) / _timeStep;

                if (_firstRun)
                {
                    errorDerivative = 0;
                    _firstRun = false;
                }

                //Compute integral term
                if (!_integralDecay)
                {
                    _errorSum += error * _timeStep;

                    //Clamp integral term
                    if (_errorSum > _upperBound)
                        _errorSum = _upperBound;
                    else if (_errorSum < _lowerBound)
                        _errorSum = _lowerBound;
                }
                else
                {
                    _errorSum = _errorSum * (1.0 - _integralDecayRatio) + error * _timeStep;
                }

                //Store this error as last error
                _lastError = error;

                //Construct output
                this.Value = _kP * error + _kI * _errorSum + _kD * errorDerivative;
                return this.Value;
            }

            public void Reset()
            {
                _errorSum = 0;
                _lastError = 0;
                _firstRun = true;
            }
        }

        public class Gyroscope
        {
            //Hellothere_1's Fast Gyroscope Adjustment Code
            public IMyGyro gyro;
            public IMyTerminalBlock refer;
            private PID yaw_pid = new PID(proportionalGain, integralGain, derivativeGain, integralDecayFactor, timeStep);
            private PID pitch_pid = new PID(proportionalGain, integralGain, derivativeGain, integralDecayFactor, timeStep);
            private PID roll_pid = new PID(proportionalGain, integralGain, derivativeGain, integralDecayFactor, timeStep);

            private int[] conversionVector = new int[3];

            public Gyroscope(IMyGyro gyroscope, IMyTerminalBlock reference)
            {
                gyro = gyroscope;
                refer = reference;

                for (int i = 0; i < 3; i++)
                {
                    Vector3D vectorShip = GetAxis(i, reference);

                    for (int j = 0; j < 3; j++)
                    {
                        double dot = vectorShip.Dot(GetAxis(j, gyro));

                        if (dot > 0.9)
                        {
                            conversionVector[j] = i;
                            break;
                        }
                        if (dot < -0.9)
                        {
                            conversionVector[j] = i + 3;
                            break;
                        }
                    }
                }
            }

            public void SetRotation(float[] rotationVector)
            {
                gyro.Pitch = rotationVector[conversionVector[0]];
                gyro.Yaw = rotationVector[conversionVector[1]];
                gyro.Roll = rotationVector[conversionVector[2]];
            }

            private Vector3D GetAxis(int dimension, IMyTerminalBlock block)
            {
                switch (dimension)
                {
                case 0: return block.WorldMatrix.Right;
                case 1: return block.WorldMatrix.Up;
                default: return block.WorldMatrix.Backward;
                }
            }

            public bool GotToOrientation(Matrix target)
            {
                Matrix current = remote.WorldMatrix;

                const double half_pi = Math.PI / 2.0;

                // Do product of normalized vector is cos between the vectors
                current.Forward.Normalize();
                current.Right.Normalize();
                current.Up.Normalize();

                target.Forward.Normalize();
                target.Up.Normalize();

                double cosx1x0 = current.Forward.Dot(target.Forward);
                double cosy1x0 = current.Right.Dot(target.Forward);
                double cosz1x0 = current.Up.Dot(target.Forward);
                double cosy1z0 = current.Right.Dot(target.Up);
                double cosz1z0 = current.Up.Dot(target.Up);

                // Target angle between all those is 90 deg
                double yaw_rad_err = half_pi - Math.Acos(cosy1x0);
                double pitch_rad_err = half_pi - Math.Acos(cosz1x0);
                double roll_rad_err = Math.Acos(cosy1z0) - half_pi;

                var yaw_ctrl = (float)yaw_pid.Control(yaw_rad_err);
                var pitch_ctrl = (float)pitch_pid.Control(pitch_rad_err);
                var roll_ctrl = (float)roll_pid.Control(roll_rad_err);

                float[] controls = new float[] { -pitch_ctrl,  yaw_ctrl, -roll_ctrl,
                                                  pitch_ctrl, -yaw_ctrl,  roll_ctrl };

                SetRotation(controls);

                double errx = Math.Abs(1.0 - cosx1x0);
                double errz = Math.Abs(1.0 - cosz1z0);
                const double epsilon = 0.0001; // below 1 deg
                if (errx < epsilon && errz < epsilon)
                {
                    // Reached target, in case PID doesn't stop (0_0)
                    gyro.GyroOverride = false;
                    return true;
                }

                gyro.GyroOverride = true;
                return false;
            }
        }

        static List<Gyroscope> gyroscopes = new List<Gyroscope>();
        static int count = 0;
        static Matrix last_orientation = new Matrix();
        static IMyTerminalBlock remote;
        public Program()
        {
            //Runtime.UpdateFrequency = UpdateFrequency.Update1;
            List<IMyGyro> gyroblocks = new List<IMyGyro>();
            GridTerminalSystem.GetBlocksOfType<IMyGyro>(gyroblocks);

            remote = GridTerminalSystem.GetBlockWithName(reference_block);

            foreach (var gyroblock in gyroblocks)
            {
                gyroscopes.Add(new Gyroscope(gyroblock, remote));
            }

            last_orientation = remote.WorldMatrix;
            Runtime.UpdateFrequency = UpdateFrequency.Once;
        }

        static string Serialize(Matrix mat)
        {
            StringBuilder sb = new StringBuilder("[");
            for (int row = 0; row < 4; ++row)
            {
                for (int col = 0; col < 4; ++col)
                {
                    sb.Append(mat[row, col]).Append(";");
                }
            }
            sb.Append("]");
            return sb.ToString();
        }

        static Matrix Deserialize(string str)
        {
            Matrix mat = Matrix.Zero;
            // Remove the brackets and last semicolumn
            var substr = str.Substring(1, str.Length - 3);

            // Separate the tokens
            var tokens = substr.Split(';');
            if (tokens.Length == 16)
            {
                for (int row = 0; row < 4; ++row)
                {
                    for (int col = 0; col < 4; ++col)
                    {
                        mat[row, col] = Convert.ToSingle(tokens[row * 4 + col]);
                    }
                }
            }
            return mat;
        }

        public void Main(string argument, UpdateType updateSource)
        {
            Echo("Starting reorientation...");
            Matrix current_orientation = remote.WorldMatrix;

            Echo("Current F: " + current_orientation.Forward.ToString());
            Echo("Target F: " + last_orientation.Forward.ToString());

            Echo("Current U: " + current_orientation.Up.ToString());
            Echo("Target U: " + last_orientation.Up.ToString());

            Echo("Iteration #" + count.ToString());
            foreach (var gyroscope in gyroscopes)
            {
                if (gyroscope.GotToOrientation(last_orientation))
                {
                    Echo(gyroscope.gyro.DisplayName + " Reached destination");
                    Runtime.UpdateFrequency = UpdateFrequency.None;
                }
                else
                {
                    Runtime.UpdateFrequency = UpdateFrequency.Update1;
                }
            }

            var str = Serialize(current_orientation);
            Echo("Sending: " + str.Replace(';','\n'));
            var antena = GridTerminalSystem.GetBlockWithName("Antenna") as IMyRadioAntenna;
            antena.TransmitMessage(str);

            if (updateSource == UpdateType.Antenna)
            {
                var display = GridTerminalSystem.GetBlockWithName("Display") as IMyTextPanel;
                display.WritePublicText(argument);
                last_orientation = Deserialize(argument);
                display.ShowPublicTextOnScreen();
            }
            else
            {
                ++count;
            }
        }


        #region post-script
    }
}
#endregion