#region pre-script
#if DEBUG
using System;
using System.Collections.Generic;
using VRageMath;
using VRage.Game;
using VRage.Library;
using System.Text;
using Sandbox.ModAPI.Interfaces;
using Sandbox.ModAPI.Ingame;
using Sandbox.Common;
using Sandbox.Game;
using VRage.Collections;
using VRage.Game.ModAPI.Ingame;
using SpaceEngineers.Game.ModAPI.Ingame;

namespace SE_Scripting
{
    public class Program : MyGridProgram
    {
#endif
        #endregion
        //=======================================================================================
        //////////////////////////START//////////////////////////////////////////
        //=======================================================================================

        string shipControllerNameTag = "Reference";

        double desiredSpeed = 5; // meters per second

        //PID Controller constants
        double proportionalGain = 5;
        double integralGain = 0;
        double derivativeGain = 2;

        PID frontBackPID;
        PID leftRightPID;
        PID upDownPID;

        Vector3D desiredDirection = new Vector3D(0, 0, 0);

        Program()
        {
            frontBackPID = new PID(proportionalGain, integralGain, derivativeGain, -10, 10, 1d/60d);
            leftRightPID = new PID(proportionalGain, integralGain, derivativeGain, -10, 10, 1d / 60d);
            upDownPID = new PID(proportionalGain, integralGain, derivativeGain, -10, 10, 1d / 60d);
        }

        void Main(string arg)
        {
            bool isSetup = GrabBlocks();
            if (!isSetup)
                return;

            Echo("It's workingggg!!!");

            switch (arg.ToLower())
            {
                case "front":
                    desiredDirection = controller.WorldMatrix.Forward;
                    break;
                case "back":
                    desiredDirection = controller.WorldMatrix.Backward;
                    break;
                case "left":
                    desiredDirection = controller.WorldMatrix.Left;
                    break;
                case "right":
                    desiredDirection = controller.WorldMatrix.Right;
                    break;
                case "up":
                    desiredDirection = controller.WorldMatrix.Up;
                    break;
                case "down":
                    desiredDirection = controller.WorldMatrix.Down;
                    break;
                case "stop":
                    desiredDirection = Vector3D.Zero;
                    break;
                default:
                    ParseArgument(arg);
                    break;
            }

            MoveInDirection(desiredDirection, desiredSpeed, controller);
        }

        void ParseArgument(string arg)
        {
            //do stuff
        }

        void MoveInDirection(Vector3D desiredDirection, double desiredSpeed, IMyShipController controller)
        {
            var currentVelocity = controller.GetShipVelocities().LinearVelocity;
            var desiredVelocity = desiredDirection * desiredSpeed;
            var relativeVelocity = desiredVelocity - currentVelocity; Echo($"RelVel: {relativeVelocity.Length()}");

            var forwardError = ScalarProjection(relativeVelocity, controller.WorldMatrix.Forward);
            var leftError = ScalarProjection(relativeVelocity, controller.WorldMatrix.Left);
            var upError = ScalarProjection(relativeVelocity, controller.WorldMatrix.Up);

            var forwardOutput = frontBackPID.Control(forwardError);
            var leftOutput = leftRightPID.Control(leftError);
            var upOutput = upDownPID.Control(upError);

            Echo($"Forward: {forwardOutput}\nLeft: {leftOutput}\nUp: {upOutput}");

            ThrustToTargetSpeed(forwardThrust, backwardThrust, forwardOutput);
            ThrustToTargetSpeed(leftThrust, rightThrust, leftOutput);
            ThrustToTargetSpeed(upThrust, downThrust, upOutput);
        }

        void ThrustToTargetSpeed(List<IMyThrust> positiveThrust, List<IMyThrust> negativeThrust, double desiredThrust)
        {
            if (desiredThrust < 0)
                ApplyThrust(negativeThrust, -desiredThrust);
            else
                ApplyThrust(positiveThrust, desiredThrust);
        }

        void ApplyThrust(List<IMyThrust> thrusters, double thrust)
        {
            foreach (var block in thrusters)
            {
                block.SetValue("Override", (float)thrust);
                Echo($"{block.CustomName}\nThrust: {thrust}");
            }
        }

        Vector3D VectorProjection(Vector3D a, Vector3D b)
        {
            if (Vector3D.IsZero(b))
                return Vector3D.Zero;

            return a.Dot(b) / b.LengthSquared() * b;
        }

        double ScalarProjection(Vector3D a, Vector3D b)
        {
            if (Vector3D.IsZero(b))
                return 0;

            return a.Dot(b) / b.Length() * Math.Sign(a.Dot(b));
        }

        List<IMyShipController> shipControllers = new List<IMyShipController>();
        List<IMyThrust> allThrust = new List<IMyThrust>();
        List<IMyThrust> upThrust = new List<IMyThrust>();
        List<IMyThrust> downThrust = new List<IMyThrust>();
        List<IMyThrust> leftThrust = new List<IMyThrust>();
        List<IMyThrust> rightThrust = new List<IMyThrust>();
        List<IMyThrust> forwardThrust = new List<IMyThrust>();
        List<IMyThrust> backwardThrust = new List<IMyThrust>();
        IMyShipController controller;

        bool GrabBlocks()
        {
            upThrust.Clear();
            downThrust.Clear();
            leftThrust.Clear();
            rightThrust.Clear();
            forwardThrust.Clear();
            backwardThrust.Clear();

            GridTerminalSystem.GetBlocksOfType(shipControllers, x => x.CustomName.Contains(shipControllerNameTag));
            GridTerminalSystem.GetBlocksOfType(allThrust);

            if (shipControllers.Count == 0)
            {
                Echo($"Error: No ship controllers named \"{shipControllerNameTag}\" found");
                return false;
            }

            controller = shipControllers[0];

            foreach (var block in allThrust)
            {
                if (controller.WorldMatrix.Forward == block.WorldMatrix.Backward)
                    forwardThrust.Add(block);
                else if (controller.WorldMatrix.Backward == block.WorldMatrix.Backward)
                    backwardThrust.Add(block);
                else if (controller.WorldMatrix.Left == block.WorldMatrix.Backward)
                    leftThrust.Add(block);
                else if (controller.WorldMatrix.Right == block.WorldMatrix.Backward)
                    rightThrust.Add(block);
                else if (controller.WorldMatrix.Up == block.WorldMatrix.Backward)
                    upThrust.Add(block);
                else if (controller.WorldMatrix.Down == block.WorldMatrix.Backward)
                    downThrust.Add(block);
            }
            return true;
        }

        //Whip's PID controller class v2 - 7/29/17
        public class PID
        {
            double _kP = 0;
            double _kI = 0;
            double _kD = 0;
            double _lowerBound = 0;
            double _upperBound = 0;
            double _timeStep = 0;
            double _errorSum = 0;
            double _lastError = 0;
            bool _firstRun = true;

            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;
            }

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

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

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

                //Compute integral term
                _errorSum += error * _timeStep;

                //Clamp integral term
                if (_errorSum > _upperBound)
                    _errorSum = _upperBound;
                else if (_errorSum < _lowerBound)
                    _errorSum = _lowerBound;

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

                //Construct output
                return _kP * error + _kI * _errorSum + _kD * errorDerivative;
            }

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

        //=======================================================================================
        //////////////////////////END//////////////////////////////////////////
        //=======================================================================================
        #region post-script
#if DEBUG
    }
}
#endif
#endregion