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;

namespace IngameScript
{
    partial class Program : MyGridProgram
    {   
        enum State
        {
            DEFAULT,
            PACKING,
            UNPACKING,
        }

        const float EPSILON = 0.05f;
        const float EPSILON_RAD = (float)(Math.PI / 64);

        State currentState = State.DEFAULT;

        IMyMotorAdvancedStator rotor01;
        IMyMotorAdvancedStator rotor02;
        IMyMotorAdvancedStator hinge01;
        IMyMotorAdvancedStator hinge02;
        IMyMotorAdvancedStator hinge03;
        IMyExtendedPistonBase piston01;

        List<IMyExtendedPistonBase> pistons = new List<IMyExtendedPistonBase>();
        List<string> pistonsNames = new List<string> { "Piston 02", "Piston 03", "Piston 04", "Piston 05", "Piston 06", "Piston 07", };

        IMyLandingGear magneticPlateArm;
        IMyLandingGear lockPlate;
        IMyExtendedPistonBase lockPiston;

        IMyTextSurface output;

        public void Print(string text)
        {
            this.output.WriteText((this.output.GetText() != "" ? Environment.NewLine : "") + text, true);
        }

        public void PrintError(string text)
        {
            this.Print($"Error: {text}");
        }

        public Program()
        {
            this.output = this.Me.GetSurface(0);
            this.output.ContentType = ContentType.TEXT_AND_IMAGE;
            this.output.WriteText("");

            this.Me.CustomData = "Packing";

            this.Print("Mining rover system starting...");

            var stators = new List<IMyMotorAdvancedStator>();
            GridTerminalSystem.GetBlocksOfType(stators);

            foreach (var s in stators)
            {
                if (s.DisplayNameText.Contains("Rotor 01"))
                    this.rotor01 = s;
                else if (s.DisplayNameText.Contains("Rotor 02"))
                    this.rotor02 = s;
                else if (s.DisplayNameText.Contains("Hinge 01"))
                    this.hinge01 = s;
                else if (s.DisplayNameText.Contains("Hinge 02"))
                    this.hinge02 = s;
                else if (s.DisplayNameText.Contains("Hinge 03"))
                    this.hinge03 = s;
            }

            var pistons = new List<IMyExtendedPistonBase>();
            GridTerminalSystem.GetBlocksOfType(pistons);

            foreach (var p in pistons)
            {
                if (p.DisplayNameText.Contains("Piston 01"))
                {
                    this.piston01 = p;
                }
                else if (p.DisplayNameText.Contains("Lock Piston"))
                {
                    this.lockPiston = p;
                }
                else
                {
                    foreach (var name in pistonsNames)
                    {
                        if (p.DisplayNameText.Contains(name))
                        {
                            this.pistons.Add(p);
                            break;
                        }
                    }
                }
            }

            var magneticPlates = new List<IMyLandingGear>();
            GridTerminalSystem.GetBlocksOfType(magneticPlates);
            foreach (var magneticPlate in magneticPlates)
            {
                if (magneticPlate.DisplayNameText.Contains("Magnetic Plate arm"))
                {
                    this.magneticPlateArm = magneticPlate;
                }
                else if (magneticPlate.DisplayNameText.Contains("Lock Plate"))
                {                    
                    this.lockPlate = magneticPlate;
                }
            }

            if (this.rotor01 == null)
            {
                this.PrintError("Can't find 'Rotor 01'");
                return;
            }

            if (this.rotor02 == null)
            {
                this.PrintError("Can't find 'Rotor 02'");
                return;
            }

            if (this.hinge01 == null)
            {
                this.PrintError("Can't find 'Hinge 01'");
                return;
            }

            if (this.hinge02 == null)
            {
                this.PrintError("Can't find 'Hinge 02'");
                return;
            }

            if (this.magneticPlateArm == null)
            {
                this.PrintError("Can't find 'Magnetic Plate arm'");
                return;
            }

            if (this.lockPlate == null)
            {
                this.PrintError("Can't find 'Lock Plate'");
                return;
            }

            if (this.piston01 == null)
            {
                this.PrintError("Can't find 'Piston 01'");
                return;
            }

            if (this.lockPiston == null)
            {
                this.PrintError("Can't find 'Lock Piston'");
                return;
            }


            this.Runtime.UpdateFrequency = UpdateFrequency.Update100;

            this.Print("Mining rover system has started");
        }

        public void Save()
        {
        }

        /// <summary>
        /// Always returns an angle between 0 and pi/2.
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <returns></returns>
        float SumAngles(float a1, float a2)
        {
            var r = (float)((a1 + a2) % (2 * Math.PI));

            if (r < 0)
                return (float)(r + 2 * Math.PI);
            else
                return r;
        }

        // Return 'true' if the rotor is in place.
        bool SetRotorVelocity(float targetAngle, IMyMotorAdvancedStator rotor, float tolerance = (float)(Math.PI / 64))
        {
            //if (rotor.Angle)
            return false;
        }

        void UpdateState()
        {
            if (this.currentState == State.UNPACKING)
            {
                if (!this.piston01.Enabled)
                {
                    this.piston01.Enabled = true;
                    foreach (var p in this.pistons)
                        p.Enabled = true;
                }

                bool finished = true;
                //bool roverLocked = false;

                if (this.magneticPlateArm.IsLocked)
                    this.magneticPlateArm.Unlock();

                this.rotor01.TargetVelocityRPM = 0.0f;

                if (this.lockPiston.CurrentPosition <= EPSILON)
                {
                    finished = false;
                    this.lockPiston.Velocity = 0.5f;
                } 
                else if (this.lockPiston.CurrentPosition >= this.lockPiston.MaxLimit - EPSILON)
                {
                    //roverLocked = true;
                    if (!this.lockPlate.IsLocked)
                        this.lockPlate.Lock();
                    this.lockPiston.Velocity = 0.0f;
                }

                if (this.hinge01.Angle <= -Math.PI / 8)
                {
                    this.hinge01.TargetVelocityRPM = 0.0f;
                }
                else // if (roverLocked)
                {
                    this.hinge01.TargetVelocityRPM = -1.5f;
                    finished = false;
                }

                if (this.hinge02.Angle <= -Math.PI / 3)
                {
                    this.hinge02.TargetVelocityRPM = 0.0f;
                }
                else // if (roverLocked)
                {
                    this.hinge02.TargetVelocityRPM = -1.5f;
                    finished = false;
                }

                if (this.piston01.CurrentPosition <= this.piston01.MaxLimit - EPSILON /*&& roverLocked*/)
                {
                    this.piston01.Velocity = 1.0f;
                    finished = false;
                }
                else
                {
                    this.piston01.Velocity = 0.0f;
                }

                if (finished)
                {
                    this.rotor01.RotorLock = false;
                    this.currentState = State.DEFAULT;
                    this.Print("Unpacking terminated");
                }
            }
            else if (this.currentState == State.PACKING)
            {
                bool finished = true;

                var rotor01InPlace = this.rotor01.Angle >= 2 * Math.PI - EPSILON_RAD || this.rotor01.Angle <= EPSILON_RAD;
                if (rotor01InPlace)
                {
                    this.rotor01.RotorLock = true;
                    this.rotor01.TargetVelocityRPM = 0.0f;
                }
                else if (this.rotor01.Angle >= Math.PI)
                {
                    this.rotor01.TargetVelocityRPM = 1.0f;
                    finished = false;
                }
                else // if (this.rotor01.Angle <= Math.PI)
                {
                    this.rotor01.TargetVelocityRPM = -1.0f;
                    finished = false;
                }

                //this.Print($"Hinge 01 angle: {this.hinge01.Angle}");
                if (this.hinge01.Angle <= Math.PI / 4.0 || rotor01InPlace && this.hinge01.Angle <= Math.PI / 2.0 - Math.PI / 32.0)
                {
                    this.hinge01.TargetVelocityRPM = 1.5f;
                    finished = false;
                }
                else
                {
                    this.hinge01.TargetVelocityRPM = 0f;
                }

                if (this.hinge02.Angle <= -Math.PI / 32.0)
                {
                    this.hinge02.TargetVelocityRPM = 1.5f;
                    finished = false;
                }
                else
                {
                    this.hinge02.TargetVelocityRPM = 0f;
                }

                if (this.piston01.CurrentPosition >= EPSILON)
                {
                    this.piston01.Velocity = -1.0f;
                    finished = false;
                }
                else
                {
                    this.piston01.Velocity = 0.0f;
                }

                //var rotor2TargetAngle = 2 * Math.PI - Math.PI / 4; // 315°.                

                if (finished)
                {
                    if (!this.magneticPlateArm.IsLocked)
                        this.magneticPlateArm.Lock();

                    if (this.lockPlate.IsLocked)
                    {
                        this.lockPlate.Unlock();
                        this.lockPiston.Velocity = -0.5f;
                    }

                    this.piston01.Enabled = false;
                    foreach (var p in this.pistons)
                        p.Enabled = false;

                    this.currentState = State.DEFAULT;
                    this.Print("Packing terminated");
                }
            }
        }

        /*
        enum RotateDirection
        {
            RIGHT,
            LEFT,
        }

        enum ElevationDirection
        {
            UP,
            DOWN,
        }

        const double SPEED_ROTATION = 1.0;
        const double SPEED_HINGE = 1.0;
        const double SPEED_EXTEND_1 = 1.0;
        const double SPEED_EXTEND_2 = 1.0;

        void Rotate(RotateDirection direction)
        {
            //if (this.rotor01.)
        }

        void ChangeElevation(ElevationDirection direction)
        {

        }

        void LockUnlock()
        {
            if (rotor01.Base)
        }*/

        public void Main(string argument, UpdateType updateSource)
        {
            if ((updateSource & UpdateType.Update100) != 0)
            {
                this.UpdateState();
            }
            else if ((updateSource & (UpdateType.Terminal | UpdateType.Trigger)) != 0)
            {
                switch (argument)
                {
                    case "UNPACK":
                        this.Print("Unpacking...");
                        this.currentState = State.UNPACKING;
                        break;

                    case "PACK":
                        this.Print("Packing...");
                        this.currentState = State.PACKING;
                        break;

                    case "STOP":
                        this.currentState = State.DEFAULT;
                        break;

                    /*
                    case "LOCK OR UNLOCK":
                        LockUnlock();
                        break;

                    case "DOWN":
                        ChangeElevation(ElevationDirection.DOWN);
                        break;

                    case "UP":
                        ChangeElevation(ElevationDirection.UP);
                        break;

                    case "LEFT":
                        Rotate(RotateDirection.LEFT);
                        break;

                    case "RIGHT":
                        Rotate(RotateDirection.RIGHT);
                        break;*/

                    default:
                        Echo($"Uknown command: {argument}");
                        break;
                }
            }
        }
    }
}