#include <iostream>
#include <cmath>
#include <vector>

// 2D coordinate point structure
struct Vector2 {
    float x, y;
    Vector2(float x = 0, float y = 0) : x(x), y(y) {}
    
    // Calculate distance between two points
    float Distance(const Vector2& other) const {
        return std::sqrt(std::pow(x - other.x, 2) + std::pow(y - other.y, 2));
    }
    
    // Vector subtraction
    Vector2 operator-(const Vector2& other) const {
        return Vector2(x - other.x, y - other.y);
    }
    
    // Vector magnitude
    float Magnitude() const {
        return std::sqrt(x * x + y * y);
    }
    
    // Vector normalization
    Vector2 Normalized() const {
        float mag = Magnitude();
        if (mag > 0) {
            return Vector2(x / mag, y / mag);
        }
        return Vector2(0, 0);
    }
};

// Target class
class Target {
public:
    Vector2 position;
    float radius; // Target radius (snap range)
    bool isActive;
    
    Target(Vector2 pos, float r) : position(pos), radius(r), isActive(true) {}
};

// AI Auto Aim Assist System
class AutoAimAssist {
private:
    Vector2 currentAim; // Current aiming position
    float assistStrength; // Assist strength (0.0 - 1.0)
    float maxAssistAngle; // Maximum snap angle (radians)
    std::vector<Target> targets;

public:
    AutoAimAssist(float strength = 0.5f, float maxAngle = 0.3f) 
        : assistStrength(strength), maxAssistAngle(maxAngle) {}
    
    // Add target
    void AddTarget(const Target& target) {
        targets.push_back(target);
    }
    
    // Clear all targets
    void ClearTargets() {
        targets.clear();
    }
    
    // Set assist strength
    void SetAssistStrength(float strength) {
        assistStrength = std::max(0.0f, std::min(1.0f, strength));
    }
    
    // Calculate angle between aim direction and target direction
    float CalculateAngle(const Vector2& aimDir, const Vector2& targetDir) {
        float dot = aimDir.x * targetDir.x + aimDir.y * targetDir.y;
        return std::acos(std::max(-1.0f, std::min(1.0f, dot)));
    }
    
    // Auto snap assist aiming calculation
    Vector2 CalculateAimPosition(const Vector2& playerPos, const Vector2& inputAim) {
        if (targets.empty()) {
            return inputAim;
        }
        
        Vector2 bestAim = inputAim;
        float closestAngle = maxAssistAngle;
        Target* bestTarget = nullptr;
        
        // Calculate input aim direction
        Vector2 inputDirection = (inputAim - playerPos).Normalized();
        
        // Find the most suitable target
        for (auto& target : targets) {
            if (!target.isActive) continue;
            
            // Calculate direction to target
            Vector2 targetDirection = (target.position - playerPos).Normalized();
            
            // Calculate angle
            float angle = CalculateAngle(inputDirection, targetDirection);
            
            // Check if within snap range
            if (angle <= maxAssistAngle) {
                // Select target with smallest angle
                if (angle < closestAngle) {
                    closestAngle = angle;
                    bestTarget = &target;
                }
            }
        }
        
        // If suitable target found, apply snap effect
        if (bestTarget != nullptr) {
            Vector2 targetDirection = (bestTarget->position - playerPos).Normalized();
            
            // Interpolate final aim direction based on assist strength
            Vector2 assistedDirection = Vector2(
                inputDirection.x * (1.0f - assistStrength) + targetDirection.x * assistStrength,
                inputDirection.y * (1.0f - assistStrength) + targetDirection.y * assistStrength
            ).Normalized();
            
            // Calculate final aim position (maintain original distance)
            float aimDistance = (inputAim - playerPos).Magnitude();
            bestAim = Vector2(
                playerPos.x + assistedDirection.x * aimDistance,
                playerPos.y + assistedDirection.y * aimDistance
            );
        }
        
        currentAim = bestAim;
        return bestAim;
    }
    
    // Get current aim position
    Vector2 GetCurrentAim() const {
        return currentAim;
    }
};

// Example usage
int main() {
    // Create auto aim assist system
    AutoAimAssist aimAssist(0.7f, 0.4f); // 70% strength, 0.4 radian max angle
    
    // Player position
    Vector2 playerPos(0, 0);
    
    // Add some targets
    aimAssist.AddTarget(Target(Vector2(10, 5), 2.0f));
    aimAssist.AddTarget(Target(Vector2(8, 8), 1.5f));
    aimAssist.AddTarget(Target(Vector2(15, 3), 2.5f));
    
    // Simulate player input aim position
    Vector2 inputAim(12, 10);
    
    // Calculate assisted aim position
    Vector2 assistedAim = aimAssist.CalculateAimPosition(playerPos, inputAim);
    
    std::cout << "Original Aim Position: (" << inputAim.x << ", " << inputAim.y << ")" << std::endl;
    std::cout << "Assisted Aim Position: (" << assistedAim.x << ", " << assistedAim.y << ")" << std::endl;
    
    return 0;
}