using System;

using System.Numerics;

using Matrix = System.Numerics.Matrix4x4;

public class Program

{

    static Vector2 startLocation = new Vector2(14.0f, 10.0f);

    static float startRotation = 0f;

    static float rotationAdjustment = 0f;

    static Vector2 endLocation;

    static float endRotation;

    static Vector2 rotationPoint;

    public static void Main()

    {

        RotateAndGetNewLocationPoint(startLocation, startRotation, rotationAdjustment, out endLocation, out endRotation, out rotationPoint);

        ConsoleDrawAndInput();

    }

    public static void RotateAndGetNewLocationPoint(Vector2 startLocation, float startRotation, float rotationAdjustment,

        out Vector2 endLocation, out float endRotation, out Vector2 rotationPoint)

    {

        Vector2 offset = new Vector2(0.0f, -5.0f); // Offset from rotation point to location point when facing north

        float sin = (float)Math.Sin(startRotation);

        float cos = (float)Math.Cos(startRotation);

        Vector2 startOffsetRotated = new Vector2((sin * offset.Y) - (cos * offset.X), (sin * offset.X) + (cos * offset.Y));

        rotationPoint = startLocation - startOffsetRotated;

        endRotation = (startRotation + rotationAdjustment) % (2 * (float)Math.PI);

        sin = (float)Math.Sin(endRotation);

        cos = (float)Math.Cos(endRotation);

        Vector2 endOffsetRotated = new Vector2((sin * offset.Y) - (cos * offset.X), (sin * offset.X) + (cos * offset.Y));

        endLocation = rotationPoint + endOffsetRotated;

    }

#region Console Draw & Input

    public static void ConsoleDrawAndInput()

    {

        Console.WriteLine("Location Point " + startLocation.ToString() + ", Rotation " + startRotation + " Radians");

        Console.WriteLine(" ");

        SetupMap();

        DrawUnit(rotationPoint, startRotation);

        DrawMap();

        Console.WriteLine(" ");

        while (true)

        {

            rotationAdjustment = 0f;

            Console.WriteLine("Enter +/- adjustment values on keyboard and press enter");

            Console.WriteLine("The format must be as below with the angle in degrees");

            Console.WriteLine("x:value y:value a:value");

            Console.WriteLine("");

            string readLine = Console.ReadLine();

            string[] commands = readLine.Split(' ');

            for (int i = 0; i < commands.Length; i++)

            {

                string[] command = (commands[i]).Split(':');

                string commandType = command[0];

                float commandValue = Convert.ToSingle(command[1]);

                if (commandType == "x")

                    endLocation.X += commandValue;

                if (commandType == "y")

                    endLocation.Y += commandValue;

                if (commandType == "a")

                    rotationAdjustment = (commandValue / 180f) * (float)Math.PI;

            }

            Console.WriteLine(" ");

            Console.WriteLine(" ");

            RotateAndGetNewLocationPoint(endLocation, endRotation, rotationAdjustment, out endLocation, out endRotation, out rotationPoint);

            Console.WriteLine("Location Point " + endLocation.ToString() + ", Rotation " + endRotation + " Radians, " +

                "Rotation Point " + rotationPoint.ToString());

            Console.WriteLine(" ");

            SetupMap();

            DrawUnit(rotationPoint, endRotation);

            DrawMap();

            Console.WriteLine(" ");

        }

    }

    public static int drawMapSize = 30;

    public static bool[][] drawArray;

    public static void SetupMap()

    {

        drawArray = new bool[drawMapSize][];

        for (int i = 0; i < drawArray.Length; i++)

            drawArray[i] = new bool[drawMapSize];

    }

    public static void DrawUnit(Vector2 centerPoint, float rotation)

    {

        Matrix rotationMatrix = Matrix.CreateRotationZ(rotation);

        Vector2 topLeft = new Vector2(-1, -3);

        Vector2 topRight = new Vector2(1, -3);

        Vector2 bottomLeft = new Vector2(-1, 3);

        Vector2 bottomRight = new Vector2(1, 3);

        Vector2 arrowClose = new Vector2(0, -3);

        Vector2 arrowFar = new Vector2(0, -5);

        DrawLine(drawArray, topLeft, topRight, centerPoint, rotationMatrix);

        DrawLine(drawArray, topRight, bottomRight, centerPoint, rotationMatrix);

        DrawLine(drawArray, bottomRight, bottomLeft, centerPoint, rotationMatrix);

        DrawLine(drawArray, bottomLeft, topLeft, centerPoint, rotationMatrix);

        DrawLine(drawArray, arrowClose, arrowFar, centerPoint, rotationMatrix);

        DrawLine(drawArray, Vector2.Zero, Vector2.Zero + new Vector2(0.01f), centerPoint, rotationMatrix);

    }

    public static void DrawLine(bool[][] drawArray, Vector2 start, Vector2 end, Vector2 centerPoint, Matrix rotationMatrix)

    {

        start = centerPoint + Vector2.Transform(start, rotationMatrix);

        end = centerPoint + Vector2.Transform(end, rotationMatrix);

        float distance = Vector2.Distance(start, end);

        Vector2 direction = Vector2.Normalize(end - start);

        for (int i = 0; i <= (int)distance; i++)

        {

            Vector2 drawPosition = (start + (i * direction));

            int drawX = (int)Math.Round(drawPosition.X);

            int drawY = (int)Math.Round(drawPosition.Y);

            if ((drawX >= 0) && (drawX < drawArray.Length) &&

                (drawY >= 0) && (drawY < drawArray[0].Length))

                drawArray[drawX][drawY] = true;

        }

    }

    public static void DrawMap()

    {

        string lineToWrite = "   ";

        for (int x = 0; x < drawArray.Length; x++)

            lineToWrite += (((x / 10) > 0) ? (x / 10).ToString() : " ") + " ";

        Console.WriteLine(lineToWrite);

        lineToWrite = "   ";

        for (int x = 0; x < drawArray.Length; x++)

            lineToWrite += (x % 10).ToString() + " ";

        Console.WriteLine(lineToWrite);

        for (int y = 0; y < drawArray[0].Length; y++)

        {

            lineToWrite = (((y / 10) > 0) ? (y / 10).ToString() : " ");

            lineToWrite += (y % 10).ToString() + " ";

            for (int x = 0; x < drawArray.Length; x++)

                lineToWrite += (drawArray[x][y]) ? "# " : ": ";

            Console.WriteLine(lineToWrite);

        }

    }

#endregion

}