using EmptyKeys.UserInterface.Generated.DataTemplatesContracts_Bindings;

using Sandbox.Game.Entities;
using Sandbox.Game.Entities.Cube;
using Sandbox.Game.EntityComponents;
//using Sandbox.ModAPI;
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.Runtime.CompilerServices;
using System.Runtime.InteropServices;
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;

namespace IngameScript
{
    partial class Program : MyGridProgram
    {
        const float EPSILON = 0.05f;
        const double DISTANCE_BEFORE_ROTATING = 15; // [m].
        const float ROTATION_SPEED = 0.5f; // [rad/s].

        enum State
        {
            NORMAL,
            LAUNCHING,
            TURNING,
            ACCELERATING,
            TRAVELLING,
            AI,
        }
         
        State currentState = State.NORMAL;

        int tickFromStart;
        int begginningOfaccelerationTick;
        bool launchPositionSet = false;
        Vector3D launchPosition;
        Vector3D directionBeforeTurning;

        Vector3D lastPosition;
        double speed;
        double acceleration;

        readonly Output output;
        readonly IMyCubeGrid grid;

        IMyThrust forwardThruster;
        IMyThrust backwardThruster;
        IMyGyro gyroscope;
        IEnumerable<IMyThrust> thrusters;
        IMyFlightMovementBlock aiMove;
        IMyOffensiveCombatBlock aiCombat;
        IEnumerable<IMyWarhead> warheads;
        IMyGasTank gasTank;
        IMyLightingBlock light;

        public Program()
        {
            var output = this.Me.GetSurface(0);
            this.output = new Output(output);

            this.output.Print("Missile controller system starting...");

            this.grid = this.Me.CubeGrid;
            this.Runtime.UpdateFrequency = UpdateFrequency.Update10;

            this.output.Print("Missile controller system started");
        }

        bool FindElements()
        {
            if (this.forwardThruster == null)
                this.forwardThruster = this.GridTerminalSystem.GetBlock<IMyThrust>("[PM] Hydrogen Thruster 01", this.grid);
            if (this.forwardThruster == null)
            {
                this.output.Print("Error: Cannot find forward thruster");
                return false;
            }

            if (this.backwardThruster == null)
                this.backwardThruster = this.GridTerminalSystem.GetBlock<IMyThrust>("[PM] Hydrogen Thruster 06", this.grid);
            if (this.backwardThruster == null)
            {
                this.output.Print("Error: Cannot find backward thruster");
                return false;
            }

            if (this.gyroscope == null)
                this.gyroscope = this.GridTerminalSystem.GetBlock<IMyGyro>("[PM] Gyroscope", this.grid);
            if (this.gyroscope == null)
            {
                this.output.Print("Error: Cannot find gyroscope");
                return false;
            }

            if (this.thrusters == null)
            {
                this.thrusters = this.GridTerminalSystem.GetBlocksFromGroup<IMyThrust>("[PM] Thrusters", this.grid);
                if (this.thrusters.Count() == 0)
                {
                    this.output.Print("Error: Cannot find thrusters");
                    return false;
                }
            }

            if (this.aiMove == null)
                this.aiMove = this.GridTerminalSystem.GetBlock<IMyFlightMovementBlock>("[PM] AI Flight (Move)", this.grid);
            if (this.aiMove == null)
            {
                this.output.Print("Error: Cannot find AI move");
                return false;
            }

            if (this.aiCombat == null)
                this.aiCombat = this.GridTerminalSystem.GetBlock<IMyOffensiveCombatBlock>("[PM] AI Offensive (Combat)", this.grid);
            if (this.aiCombat == null)
            {
                this.output.Print("Error: Cannot find AI combat");
                return false;
            }

            if (this.warheads == null)
                this.warheads = this.GridTerminalSystem.GetBlocksFromGroup<IMyWarhead>("[PM] Warheads", this.grid);
            if (this.warheads.Count() == 0)
            {
                this.output.Print("Error: Cannot find any warhead");
                return false;
            }
            // Debug.
            //else
            //{
            //    this.output.Print(String.Format("Number of warhead: {0}", this.warheads.Count()));
            //}

            if (this.gasTank == null)
                this.gasTank = this.GridTerminalSystem.GetBlock<IMyGasTank>("[PM] Hydrogen Tank", this.grid);
            if (this.gasTank == null)
            {
                this.output.Print("Error: Cannot find gas tank");
                return false;
            }

            if (this.light == null)
                this.light = this.GridTerminalSystem.GetBlock<IMyLightingBlock>("[PM] Light", this.grid);
            if (this.light == null)
            {
                this.output.Print("Error: Cannot find light");
                return false;
            }

            return true;
        }

        //bool BlinkBeforeBeingAutodestructed()
        //{
        //    return this.MsSinceLaunch > AUTO_DESTRUCTION_AFTER - 3000;
        //}

        //bool MustBeAutodestructed()
        //{
        //    return this.MsSinceLaunch > AUTO_DESTRUCTION_AFTER;
        //}

        bool EnemyAtRange()
        {
            return this.aiCombat.SearchEnemyComponent.FoundEnemyId.HasValue;
        }

        void UpdateState10()
        {
            this.tickFromStart += 10;

            if (this.forwardThruster != null)
            {
                var currentPosition = this.forwardThruster.GetPosition();
                if (this.lastPosition != new Vector3D())
                {
                    var d = (currentPosition - this.lastPosition).Length();
                    var speed = d * 10;
                    this.acceleration = (speed - this.speed) * 10;
                    this.speed = speed;
                    //this.output.Print($"Spd: {this.speed:0.0}, Acc: {this.acceleration:0.0}");
                }
                this.lastPosition = currentPosition;
            }

            switch (this.currentState)
            {
                case State.LAUNCHING:
                    if (!this.FindElements())
                    {
                        this.output.Print("Cannot find all missile elements, launching aborted");
                        this.currentState = State.NORMAL;
                        break;
                    }

                    if (!this.launchPositionSet)
                    {
                        this.launchPosition = this.forwardThruster.GetPosition();
                        this.launchPositionSet = true;
                    }

                    //this.forwardThruster.Enabled = true; // Only one thruster is enabled when launching. Replaced by the loop below to avoid issues when gravity is present.
                    foreach (var thruster in this.thrusters)
                    {
                        if (thruster != this.backwardThruster)
                            thruster.Enabled = true;
                    }

                    this.forwardThruster.ThrustOverridePercentage = 1f; // Can be lowered for low gravity but set to 1f for 1G planet gravity.

                    //this.output.Print($"Distance from launch: {this.DistanceFromLaunch:0.0} m");

                    if (this.DistanceFromLaunch > DISTANCE_BEFORE_ROTATING)
                    {
                        this.directionBeforeTurning = this.forwardThruster.WorldMatrix.Forward.Normalized();
                        this.gyroscope.GyroOverride = true;
                        this.gyroscope.Roll = ROTATION_SPEED;

                        this.output.Print($"Turning mode");
                        this.currentState = State.TURNING;
                    }
                    break;

                case State.TURNING:
                    var dotProduct = this.forwardThruster.WorldMatrix.Forward.Normalized().Dot(this.directionBeforeTurning);
                    //this.output.Print($"Dot product: {dotProduct:0.000}");
                    if (dotProduct <= EPSILON)
                    {
                        this.gyroscope.GyroOverride = false;
                        this.gyroscope.Roll = 0;
                        this.forwardThruster.ThrustOverridePercentage = 1f;

                        this.output.Print($"Accelerating mode");
                        this.begginningOfaccelerationTick = this.tickFromStart;
                        this.currentState = State.ACCELERATING;
                    }
                    break;

                case State.ACCELERATING:
                    //this.CheckTank();
                    this.CheckEnemies();

                    if (this.acceleration <= EPSILON)
                    {                        
                        this.forwardThruster.ThrustOverridePercentage = 0;
                        this.output.Print($"Travelling mode");
                        this.currentState = State.TRAVELLING;
                    }
                    break;

                case State.TRAVELLING:
                    //this.CheckTank();
                    this.CheckEnemies();
                    
                    break;

                case State.AI:
                    //this.CheckTank();
                    break;

                case State.NORMAL:
                    break; // Nothing;
            }
        }

        //void CheckTank()
        //{
        //    if (this.gasTank.FilledRatio <= EPSILON)
        //    {
        //        this.output.Print("Tank empty, detonating");
        //        Detonate();
        //    }
        //}

        void CheckEnemies()
        {
            if (this.EnemyAtRange())
            {
                this.forwardThruster.ThrustOverridePercentage = 0;

                foreach (var warhead in this.warheads)
                    warhead.IsArmed = true;

                this.backwardThruster.Enabled = true;
                this.aiMove.Enabled = true;

                this.output.Print("Enemy in range, switching to AI mode");
                this.currentState = State.AI;
            }
        }


        void Detonate()
        {
            foreach (var warhead in this.warheads)
                warhead.Detonate(); 
        }

        public void Save()
        {
        }

        //double MsSinceLaunch
        //{
        //    get { return (double)this.tickFromStart / 60 * 1000; }
        //}

        double DistanceFromLaunch
        {
            get { return (this.forwardThruster.GetPosition() - this.launchPosition).Length(); }
        }

        public void Main(string argument, UpdateType updateSource)
        {
            if ((updateSource & UpdateType.Update10) != 0)
            {
                this.UpdateState10();
            }
            else if ((updateSource & (UpdateType.Script | UpdateType.Terminal | UpdateType.Trigger)) != 0)
            {
                switch (argument)
                {
                    case "START":
                        this.output.Print("Launching mode");
                        this.tickFromStart = 0;
                        this.currentState = State.LAUNCHING;
                        break;

                    case "STOP":
                        this.output.Print("Stop mode");
                        this.currentState = State.NORMAL;
                        break;

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