using System;
using System.Numerics;
using Godot;
using Vector2 = Godot.Vector2;
using Vector3 = Godot.Vector3;

/// <summary>
/// </summary>
public class GroundMotionComponent
{
    public float Accel = 50;
    public float Damping = 0.2f;
    public float MaxSpeed = 8;

    
    private void CalcPlaneVelocity(float delta, Entity entity, Vector3 targetPosition)
    {
        Vector2 planeTargetVector = new Vector2(targetPosition.x - entity.GlobalTranslation.x,
            targetPosition.z - entity.GlobalTranslation.z);
        float targetDistance = planeTargetVector.Length();
        Vector2 planeTargetDirection = planeTargetVector / targetDistance;
        
        Vector2 planeVelocity = new Vector2(entity.Velocity.x, entity.Velocity.z);
        // GD.Print("-- Step: distance: " + targetDistance + " dir: " + planeTargetDirection + " speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
        planeVelocity -= planeVelocity * Damping;
        // GD.Print("  damp : speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
        
        if (targetDistance < 0.01)
        {
            planeVelocity = Vector2.Zero;
        } else {
            planeVelocity = planeVelocity.Length() * planeTargetDirection + planeTargetDirection * Accel * delta;
            // GD.Print("  accel: speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());            

            float projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
            // GD.Print("  Projected step: " + projectedStep + " Speed: " + planeVelocity.Length() + " delta: " + delta);
            if (projectedStep > targetDistance)
            {
                planeVelocity *= targetDistance / projectedStep;
                projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
                // GD.Print("  corr speed: " + planeVelocity.Length() + " step: " + projectedStep);                
            }
            
            float planeSpeed = planeVelocity.Length();
            if (planeSpeed > MaxSpeed)
            {
                planeVelocity *= MaxSpeed / planeSpeed;
            }
        }

        entity.Velocity = new Vector3(planeVelocity.x, entity.Velocity.y, planeVelocity.y);
    }


    private void CalcVerticalVelocity(float delta, Entity entity, TileWorld tileWorld)
    {
        Vector3 entityVelocity = entity.Velocity;

        Vector2 currentTile = tileWorld.WorldToOffsetCoords(entity.GlobalTransform.origin);
        Vector2 nextTile = tileWorld.WorldToOffsetCoords(entity.GlobalTransform.origin + entityVelocity * delta);

        if (nextTile != currentTile)
        {
            float currentHeight = tileWorld.GetHeightAtOffset(currentTile);
            float nextHeight = tileWorld.GetHeightAtOffset(nextTile);

            bool isOnFloor = entity.IsOnFloor();
            
            if (nextHeight - entity.GlobalTransform.origin.y > 0.1)
            {
                GD.Print("Jump!");
                entityVelocity.y = 10;
            }
        }

        entityVelocity.y -= 9.81f * delta;
        
        entity.Velocity = entityVelocity;
    }
    
    
    public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, TileWorld tileWorld)
    {
        CalcPlaneVelocity(delta, entity, targetPosition);
        
        CalcVerticalVelocity(delta, entity, tileWorld);

        Vector3 prePhysicsVelocity = entity.Velocity;
        entity.Velocity = entity.MoveAndSlide(entity.Velocity);
        GD.Print("Pre : speed: " + prePhysicsVelocity.Length() + " Velocity: " + prePhysicsVelocity);
        GD.Print("Post: speed: " + entity.Velocity.Length() + " Velocity: " + entity.Velocity);
    }
}