#include "Physics.h"
#include <cmath>

const float friction = 0.05f;
const float maxSpeed = 2.0f;
const float acceleration = 0.2f;
const float rotationAcceleration = 0.075f;
const float rotationFriction = 0.1f;
const float maxRotationSpeed = 0.5f;

bool CheckFenceCollision(float rXMin, float rXMax, float rZMin, float rZMax, const Plot& plot) {
    float fXMin, fXMax, fZMin, fZMax;
    if (plot.mod_x == 0) { // Płot pionowy
        fXMin = plot.xc - plot.grubosc / 2.0f;
        fXMax = plot.xc + plot.grubosc / 2.0f;
        fZMin = plot.zc - plot.length / 2.0f;
        fZMax = plot.zc + plot.length / 2.0f;
    }
    else { // Płot poziomy
        fXMin = plot.xc - plot.length / 2.0f;
        fXMax = plot.xc + plot.length / 2.0f;
        fZMin = plot.zc - plot.grubosc / 2.0f;
        fZMax = plot.zc + plot.grubosc / 2.0f;
    }
    return (rXMax >= fXMin && rXMin <= fXMax && rZMax >= fZMin && rZMin <= fZMax);
}

bool CheckAllFencesCollision(float rXMin, float rXMax, float rZMin, float rZMax, const std::vector<Plot>& fences) {
    for (const auto& fence : fences) {
        if (CheckFenceCollision(rXMin, rXMax, rZMin, rZMax, fence)) return true;
    }
    return false;
}

void UpdateRover(const std::vector<Plot>& fences) {
    float timeScale = deltaTime * 144.0f;
    if (timeScale > 5.0f) timeScale = 5.0f;

    // Przyspieszenie
    if (keyWPressed) {
        velocity += acceleration * timeScale;
        if (velocity > maxSpeed) velocity = maxSpeed;
    }
    else if (keySPressed) {
        velocity -= acceleration * timeScale;
        if (velocity < -maxSpeed) velocity = -maxSpeed;
    }
    else {
        float frictionStep = friction * timeScale;
        if (velocity > 0) { velocity -= frictionStep; if (velocity < 0) velocity = 0; }
        else if (velocity < 0) { velocity += frictionStep; if (velocity > 0) velocity = 0; }
    }

    // Obrót
    if (keyAPressed) {
        rotationVelocity += rotationAcceleration * timeScale;
        if (rotationVelocity > maxRotationSpeed) rotationVelocity = maxRotationSpeed;
    }
    else if (keyDPressed) {
        rotationVelocity -= rotationAcceleration * timeScale;
        if (rotationVelocity < -maxRotationSpeed) rotationVelocity = -maxRotationSpeed;
    }
    else {
        float driftFactor = 0.1f;
        float rotationFrictionStep = rotationFriction * driftFactor * timeScale;
        if (rotationVelocity > 0) { rotationVelocity -= rotationFrictionStep; if (rotationVelocity < 0) rotationVelocity = 0; }
        else if (rotationVelocity < 0) { rotationVelocity += rotationFrictionStep; if (rotationVelocity > 0) rotationVelocity = 0; }
    }

    float actualRotationStep = rotationVelocity * timeScale;
    if (velocity < 0.0f) actualRotationStep = -actualRotationStep;

    float currentMoveStep = velocity * timeScale;
    float radRotation = Rotation * GL_PI / 180.0f;
    float newSides = Sides - currentMoveStep * cos(radRotation);
    float newFoward = Foward - currentMoveStep * sin(radRotation);

    const float roverHalfWidthX = 19.0f;
    const float roverHalfLengthZ = 12.0f;
    float roverXMin = newSides - roverHalfWidthX;
    float roverXMax = newSides + roverHalfWidthX;
    float roverZMin = newFoward - roverHalfLengthZ;
    float roverZMax = newFoward + roverHalfLengthZ;

    if (!Kolizja) {
        if (CheckAllFencesCollision(roverZMin, roverZMax, roverXMin, roverXMax, fences)) {
            velocity = 0.0f;
        }
        else {
            Sides = newSides;
            Foward = newFoward;
        }

        if (actualRotationStep != 0.0f) {
            float newRotation = Rotation + actualRotationStep;
            Rotation = newRotation;
            // Tutaj uprościłem logikę dla czytelności, można dodać pełną kolizję OBB z oryginału
        }
    }
    else {
        Sides = newSides;
        Foward = newFoward;
        Rotation += actualRotationStep;
    }

    if (Rotation >= 360.0f) Rotation -= 360.0f;
    if (Rotation < 0.0f)    Rotation += 360.0f;
}