/*
 * API index: https://github.com/malware-dev/MDK-SE/wiki/Api-Index
 * Vector transformation: https://github.com/malware-dev/MDK-SE/wiki/Vector-Transformations-with-World-Matrices
 * How to get rotation/position: https://forum.keenswh.com/threads/how-do-i-get-the-world-position-and-rotation-of-a-ship.7363867/
 * 
 */

using Sandbox.Game.EntityComponents;
using Sandbox.ModAPI.Ingame;
using Sandbox.ModAPI.Interfaces;

using SpaceEngineers.Game.ModAPI.Ingame;

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

using VRage;
using VRage.Collections;
using VRage.Game;
using VRage.Game.Components;
using VRage.Game.GUI.TextPanel;
using VRage.Game.ModAPI.Ingame;
using VRage.Game.ModAPI.Ingame.Utilities;
using VRage.Game.ObjectBuilders.Definitions;

using VRageMath;
using VRageMath.Spatial;

namespace IngameScript
{
    partial class Program : MyGridProgram
    {
        const int CONSOLE_NB_LINES = 9;
        const float MAX_SPEED = 0.15f; // In rad per second.
        const int NB_OF_GYRO = 1000; // To limit the number of gyros set to overdrive.

        enum State
        {
            NORMAL, // Normal state: Manual commands.
            AUTO_STABILIZATION,
            AUTO_DOCKING,
        }

        State state = State.NORMAL;

        readonly Output output;

        IMyRemoteControl remoteController;
        IMyCubeGrid grid;
        readonly List<IMyGyro> gyros = new List<IMyGyro>();

        IMyBroadcastListener connnectorMinerPositionListener;
        MatrixD connectorMinerPosition;

        public Program()
        {
            this.grid = this.Me.CubeGrid;

            var cockpits = new List<IMyCockpit>();
            this.GridTerminalSystem.GetBlocksOfType(cockpits);
            IMyCockpit cockpit = null;
            foreach (var c in cockpits)
            {
                if (c.CubeGrid == this.grid)
                {
                    cockpit = c;
                    break;
                }
            }

            if (cockpit == null)
            {
                this.Echo("Can't find a cockpit");
                return;
            }

            this.output = new Output(cockpit, CONSOLE_NB_LINES);

            this.output.Print("Intializing navigation control...");

            var remoteControls = new List<IMyRemoteControl>();
            this.GridTerminalSystem.GetBlocksOfType(remoteControls);
            foreach (var rc in remoteControls)
            {
                if (rc.CubeGrid == this.grid)
                {
                    this.remoteController = rc;
                    break;
                }
            }

            this.GridTerminalSystem.GetBlocksOfType(
                this.gyros,
                (IMyGyro gyro) => gyro.CubeGrid == this.grid
            );

            if (this.remoteController == null)
            {
                this.output.Print("Can't find a remote controller");
                return;
            }


            this.connnectorMinerPositionListener = this.IGC.RegisterBroadcastListener("POSITION_CONNECTOR_MINER");

            /*
            var sensor = this.GridTerminalSystem.GetBlockWithName("Sensor") as IMySensorBlock;
            var entities = new List<MyDetectedEntityInfo>();
            sensor.DetectedEntities(entities);
            foreach (var entity in entities)
            {
                entity.
                this.output.Print(entity.Name);
            }*/

            this.Runtime.UpdateFrequency = UpdateFrequency.Update10;

            this.output.Print("Navigation control initializing complete");
        }

        public void Save()
        {
        }

        void UpdateStateAutoStabilization()
        {
            //var pos = this.remoteController.Position;
            var gravity = this.remoteController.GetNaturalGravity().Normalized();

            if (!Vector3D.IsZero(gravity))
            {
                var worldMatrix = this.remoteController.WorldMatrix;
                var up = worldMatrix.Up;
                var forward = worldMatrix.Forward;
                var left = worldMatrix.Left;

                //this.Print(gravity.ToString());
                //gravity.ToString();
                this.output.Display(
                    $"Gravity: {gravity.ToString("f2")}\n" +
                    $"Up: {up.ToString("f2")}\n" +
                    $"Foward: {forward.ToString("f2")}\n" +
                    $"Left: {left.ToString("f2")}\n",
                    1
                );

                var alpha = (float)Math.Asin(up.Dot(gravity));
                var beta = (float)Math.Asin(left.Dot(gravity));

                this.output.Display(
                    $"Alpha: {alpha}\n" +
                    $"Beta: {beta}\n" +
                    $"Is under control: {this.remoteController.IsUnderControl}",
                    2
                );

                for (int i = 0; i < NB_OF_GYRO && i < this.gyros.Count; i++)
                {
                    var gyro = this.gyros[i];
                    gyro.Pitch = MathHelper.Clamp(-alpha, -MAX_SPEED, MAX_SPEED);
                    gyro.Roll = MathHelper.Clamp(beta, -MAX_SPEED, MAX_SPEED);
                }
            }
        }

        void UpdateStateAutoDocking()
        {
            if (this.connnectorMinerPositionListener.HasPendingMessage)
                this.connectorMinerPosition = this.connnectorMinerPositionListener.AcceptMessage().As<MatrixD>();

            if (this.connectorMinerPosition.IsValid())
            {
                this.output.Display($"Connector position:\n{this.connectorMinerPosition}", 3);
            }
            else
            {
                this.output.Display($"NOPE", 3);
            }
        }

        void UpdateState()
        {
            switch (this.state)
            {
                case State.AUTO_STABILIZATION:
                    this.UpdateStateAutoStabilization();
                    break;
                case State.AUTO_DOCKING:
                    this.UpdateStateAutoDocking();
                    break;
                case State.NORMAL:
                    break; // Nothing.
            }
        }

        void UpdateStateMachine(State newState)
        {
            if (this.state == newState)
                return;

            if (newState == State.NORMAL)
            {
                switch (this.state)
                {
                    case State.AUTO_STABILIZATION:
                        foreach (var gyro in this.gyros)
                            gyro.GyroOverride = false;
                        this.state = State.NORMAL;
                        this.output.Print("Auto stabilization disabled");
                        break;
                    case State.AUTO_DOCKING:
                        this.state = State.NORMAL;
                        this.output.Print("Auto docking disabled");
                        break;
                    case State.NORMAL:
                        break; // Nothing.

                }
            }
            else
            {
                this.UpdateStateMachine(State.NORMAL);
                switch (newState)
                {
                    case State.AUTO_STABILIZATION:
                        for (int i = 0; i < NB_OF_GYRO && i < this.gyros.Count; i++)
                        {
                            var gyro = this.gyros[i];
                            gyro.GyroOverride = true;
                            gyro.Yaw = 0f;
                            gyro.Pitch = 0f;
                            gyro.Roll = 0f;
                        }

                        this.state = State.AUTO_STABILIZATION;
                        this.output.Print("Auto stabilization enabled");
                        break;
                    case State.AUTO_DOCKING:
                        this.state = State.AUTO_DOCKING;
                        this.output.Print("Auto docking enabled");
                        break;
                    case State.NORMAL:
                        break; // Nothing.
                }
            }
        }

        public void Main(string argument, UpdateType updateSource)
        {
            if ((updateSource & UpdateType.Update10) != 0)
            {
                this.UpdateState();
            }
            else if ((updateSource & (UpdateType.Terminal | UpdateType.Trigger)) != 0)
            {
                switch (argument)
                {
                    case "SWITCH_AUTO_STABILIZATION":
                        if (this.state != State.AUTO_STABILIZATION)
                            this.UpdateStateMachine(State.AUTO_STABILIZATION);
                        else
                            this.UpdateStateMachine(State.NORMAL);
                        break;

                    case "SWITCH_AUTO_DOCKING":
                        if (this.state != State.AUTO_DOCKING)
                            this.UpdateStateMachine(State.AUTO_DOCKING);
                        else
                            this.UpdateStateMachine(State.NORMAL);
                        break;

                    default:
                        this.output.Print($"Uknown command: {argument}");
                        break;
                }
            }
        }
    }
}