MapCollider

#include <iostream>
#include <math.h>
#include <SFML/Graphics.hpp>
#include "Map.h"
#include "MapCollider.h"
#include "MapInfo.h"
#include "MapData.h"
#include "GameData.h"
#include "AssetManager.h"
#include "Tile.h"
#include "CONFIG.h"

#include "ECS.h"
#include "PositionSystem.h"
#include "VelocitySystem.h"
#include "BoundsSystem.h"
#include "CollisionSystem.h"

#define ONLYFORONE if(col->getCompID() == 0 && MAPCOLLIDER_EXCESSIVEDEBUG_PRINTGROUNDED)

namespace station {
    MapCollider::MapCollider(Map* MAP) : _map(MAP) {
    }

    bool MapCollider::isOOB(PositionComponent* pos, BoundsComponent* bounds) {
        int width = _map->activeMap->mapWidth;
        if (pos->position.x + (bounds->bounds.x / 2) < 0) return true;
        if (pos->position.x - (bounds->bounds.x / 2) > width * TILE_WIDTH) return true;
        if (pos->position.y + (bounds->bounds.y / 2) < 0) return true;
        if (pos->position.y - (bounds->bounds.y / 2) > width * TILE_HEIGHT) return true;
        return false;
    }

    std::pair<sf::Vector2i, sf::Vector2i> MapCollider::getCollisionBounds(CollisionComponent* col) {
        PositionComponent* pos = _map->_data->ecs->positionSystem->getComp(col->getCompID());
        BoundsComponent* bounds = _map->_data->ecs->boundsSystem->getComp(col->getCompID());

        //get the edges of the tile area the collider potentially intersects with
        //-1 since tile 0,0 is at 1,1
        int left = TILE_WIDTH * floor(pos->position.x - (bounds->bounds.x / 2) - MAPCOLLIDER_TILEAREA_ADDEDAREA);
        int right = TILE_WIDTH * ceil(pos->position.x + (bounds->bounds.x / 2) - 1 + MAPCOLLIDER_TILEAREA_ADDEDAREA);
        int top = TILE_HEIGHT * floor(pos->position.y - (bounds->bounds.y / 2) - MAPCOLLIDER_TILEAREA_ADDEDAREA);
        int bottom = TILE_HEIGHT * ceil(pos->position.y + (bounds->bounds.y / 2) - 1 + MAPCOLLIDER_TILEAREA_ADDEDAREA);

        int mapWidth = _map->activeMap->mapWidth;
        int mapHeight = _map->activeMap->mapHeight;

        //push edges into map bounds
        left = clip(left, 0, mapWidth - 1);
        right = clip(right, 0, mapWidth - 1);
        top = clip(top, 0, mapHeight - 1);
        bottom = clip(bottom, 0, mapHeight - 1);

        return TileArea(sf::Vector2i(left, top), sf::Vector2i(right, bottom));
    }

    /*the map class's update function sets every colliders grounded flag to
    false, then calls this function for each collider and potentially sets
    grounded to true again*/
    /*ive tried using the entity-entity collision resolve algorithm for this too,
    but it led to unwanted behaviour and was generally too intuitive so i couldnt
    add new mechanics to it effectively, this will be enough anyway*/
    void MapCollider::resolveCollisionWithMap(CollisionComponent* col) {
        PositionComponent* pos = _map->_data->ecs->positionSystem->getComp(col->getCompID());
        BoundsComponent* bounds = _map->_data->ecs->boundsSystem->getComp(col->getCompID());
        VelocityComponent* vel = _map->_data->ecs->velocitySystem->getComp(col->getCompID());

        //check if entity is OOB, in that case dont bother with collision
        //dont remove this, this will lead to unwanted collision behaviour too
        if (isOOB(pos, bounds)) return;

        int currentTile;
        sf::FloatRect currentTileBounds;

        TileArea tileArea = getCollisionBounds(col); //first = topleft, second = bottomright
        bool onGround = false;

        for (int y = tileArea.first.y; y <= tileArea.second.y; y++) {
            if (MAPCOLLIDER_EXCESSIVEDEBUG_TILEAREA) std::cout << "Tile Area: ";
            for (int x = tileArea.first.x; x <= tileArea.second.x; x++) {
                currentTile = y * _map->activeMap->mapWidth + x;
                if (isSolid(currentTile)) {
                    //tiles surrounding this tile
                    int leftTile = currentTile >= 0 ? currentTile - 1 : -1;
                    int rightTile = currentTile <= _map->activeMap->mapWidth ? currentTile + 1 : -1;
                    int aboveTile = currentTile >= _map->activeMap->mapWidth - 1 ? currentTile - _map->activeMap->mapWidth : -1;
                    int belowTile = currentTile <= _map->activeMap->mapWidth - _map->activeMap->mapWidth ? currentTile + _map->activeMap->mapWidth : -1;
                    //are they solid? if so, dont push out into that direction
                    bool dontPushOutLeft = leftTile != -1 ? isSolid(leftTile) : false;
                    bool dontPushOutRight = rightTile != -1 ? isSolid(rightTile) : false;
                    bool dontPushOutUp = aboveTile != -1 ? isSolid(aboveTile) : false;
                    bool dontPushOutDown = belowTile != -1 ? isSolid(belowTile) : false;
                    const sf::FloatRect tilebounds = getBoundsOfTile(currentTile);

                    //if (col->getCompID() == 0) std::cout << "current tile (" << currentTile << ") is solid" << std::endl;

                    sf::Vector2f dir(pos->position.x - (x + 0.5f), pos->position.y - (y + 0.5f));
                    //overlap between the collider and the tile in the x axis
                    float x_overlap = (bounds->bounds.x * 0.5f) + (TILE_WIDTH * 0.5f) - abs(dir.x);
                    //overlap between the collider and the tile in the y axis
                    float y_overlap = (bounds->bounds.y * 0.5f) + (TILE_HEIGHT * 0.5f) - abs(dir.y);

                    //grounded?
                    sf::Vector2f vectorLeftVertex(
                        pos->position.y + (bounds->bounds.y * 0.5f) - (y + 0.5f),
                        pos->position.x - (bounds->bounds.x * 0.5f) - (x + 0.5f)
                        );
                    sf::Vector2f vectorRightVertex(
                        pos->position.y + (bounds->bounds.y * 0.5f) - (y + 0.5f),
                        pos->position.x + (bounds->bounds.x * 0.5f) - (x + 0.5f)
                    );
                    float angleLeftVertex = atan2(
                        vectorLeftVertex.y,
                        vectorLeftVertex.x) * 180.0f / PI;
                    float angleRightVertex = atan2(
                        vectorRightVertex.y,
                        vectorRightVertex.x) * 180.0f / PI;

                    ONLYFORONE std::cout << "angleLeftVertex: " << angleLeftVertex << '\n'
                        << "angleRightVertex: " << angleRightVertex << '\n';

                    //if either one of the two is inside the angle range or they are at opposite sides of the
                    //angle range, the entity is "above" the tile
                    bool angleRangeFulfilled = false;
                    if (isInAngleRange(angleLeftVertex) || isInAngleRange(angleRightVertex)) {
                        angleRangeFulfilled = true;
                        ONLYFORONE std::cout << "one of angles in range" << std::endl;
                    }
                    else if (isLeftToAngleRange(angleLeftVertex) && isRightToAngleRange(angleRightVertex)) {
                        ONLYFORONE std::cout << "angles at other side of range" << std::endl;
                        angleRangeFulfilled = true;
                    }
                    //angle < -45.0f && angle > -135.0f
                    if (x_overlap > 0 && y_overlap > -0.01f && dir.y <= 0 && !dontPushOutUp && angleRangeFulfilled) {
                        if (MAPCOLLIDER_EXCESSIVEDEBUG_PRINTGROUNDED) std::cout << "dir.y = " << dir.y << "\n";
                        onGround = true;;
                    }

                    //DEBUG: set vertices for debug lines and push them to vector for draw
                    //nothing gets drawn here, the vertex arrays just get set up
                    if (DEBUG_MAPCOLLIDER_DRAWGROUNDEDNESS) {
                        //sf::Vertex lines[6];
                        col->lines[0].position = sf::Vector2f(
                            pos->position.x - (bounds->bounds.x * 0.5f),
                            pos->position.y + (bounds->bounds.y * 0.5f)
                        );
                        col->lines[1].position = sf::Vector2f((x + 0.5f), (y + 0.5f));
                        col->lines[2].position = sf::Vector2f(
                            pos->position.x + (bounds->bounds.x * 0.5f),
                            pos->position.y + (bounds->bounds.y * 0.5f)
                        );
                        col->lines[3].position = sf::Vector2f((x + 0.5f), (y + 0.5f));
                        col->lines[4].position = sf::Vector2f(
                            pos->position.x - (bounds->bounds.x * 0.5f),
                            pos->position.y + (bounds->bounds.y * 0.5f) + 0.1f
                        );
                        col->lines[5].position = sf::Vector2f(
                            pos->position.x + (bounds->bounds.x * 0.5f),
                            pos->position.y + (bounds->bounds.y * 0.5f) + 0.1f
                        );

                        for (int i = 0; i < 2; i++) {
                            col->lines[i].color = isInAngleRange(angleLeftVertex) ? sf::Color::Green : sf::Color::Red;
                        }
                        for (int i = 2; i < 4; i++) {
                            col->lines[i].color = isInAngleRange(angleRightVertex) ? sf::Color::Green : sf::Color::Red;
                        }
                        for (int i = 4; i < 6; i++) {
                            col->lines[i].color = col->groundedOnMap ? sf::Color::Green : sf::Color::Red;
                        }

                        groundedCheckThisUpdate.emplace_back(col);
                    }

                    //intersection?
                    if (!(x_overlap < 0 || y_overlap < 0)) {
                        //prioritize y over x due to stepping if y is small enough
                        if (col->groundedOnEntity || col->groundedOnMap) {
                            if (y_overlap < MAPCOLLIDER_STEPHEIGHT && dir.y < 0) {
                                y_overlap = -1000.0f;
                                dir.y = -1000.0f;
                            }
                        }
                        else {
                            if (y_overlap < MAPCOLLIDER_STEPHEIGHT_AIRBORNE && dir.y < 0) {
                                y_overlap = -1000.0f;
                                dir.y = -1000.0f;
                            }
                        }

                        if (x_overlap < y_overlap) {
                            //when you smash into a horizontal wall, adjust y velocity
                            if (MAPCOLLIDER_IMPULSEABSORBTION_Y && !(col->groundedOnEntity || col->groundedOnMap)) {
                                float velx = abs(vel->velocity.x); //dir doesnt matter //TODO runtime crash here apparently
                                if (vel) vel->velocity.y -= abs(velx * col->onWallHitXToYConversion);
                            }
                            if (dir.x < 0) {
                                if (!dontPushOutLeft) {
                                    pos->setPosition(
                                        tilebounds.left - (bounds->bounds.x / 2) - MAPCOLLIDER_GIVE_X,
                                        pos->position.y
                                    );
                                    if (vel) vel->velocity.x = std::min(vel->velocity.x, 0.0f);
                                }
                            }
                            else {
                                if (!dontPushOutRight) {
                                    pos->setPosition(
                                        (tilebounds.left + tilebounds.width) + (bounds->bounds.x / 2) + MAPCOLLIDER_GIVE_X,
                                        pos->position.y
                                    );
                                    if (vel) vel->velocity.x = std::max(vel->velocity.x, 0.0f);
                                }
                            }
                        }
                        else {
                            if (dir.y < 0) {
                                if (!dontPushOutUp) {
                                    pos->setPosition(
                                        pos->position.x,
                                        tilebounds.top - (bounds->bounds.y / 2) - MAPCOLLIDER_GIVE_Y
                                    );
                                    if (vel) vel->velocity.y = std::min(vel->velocity.y, 0.0f);
                                }
                            }
                            else {
                                if (!dontPushOutDown) {
                                    pos->setPosition(
                                        pos->position.x,
                                        (tilebounds.top + tilebounds.height) + (bounds->bounds.y / 2) + MAPCOLLIDER_GIVE_Y
                                    );
                                    if(vel) vel->velocity.y = std::max(vel->velocity.y, 0.0f);
                                }
                            }
                        }
                    }
                    if (x_overlap > -MAPCOLLIDER_ONSIDEOFTILE_ZONE && y_overlap > 0.05f) {
                        if(dir.x < 0) col->onSideOfTile = CollisionComponent::OnSideOfTile::RIGHT;
                        if (dir.x > 0) col->onSideOfTile = CollisionComponent::OnSideOfTile::LEFT;
                    }
                }
                if (MAPCOLLIDER_EXCESSIVEDEBUG_TILEAREA) std::cout << std::to_string(currentTile) << " ";
            }
            if (MAPCOLLIDER_EXCESSIVEDEBUG_TILEAREA) std::cout << '\n';
        }
        if (MAPCOLLIDER_EXCESSIVEDEBUG_TILEAREA) std::cout << '\n';
        col->groundedOnMap = onGround;
    }

    sf::FloatRect MapCollider::getBoundsOfTile(int index) {
        return sf::FloatRect(
            sf::Vector2f((index % _map->activeMap->mapWidth) * TILE_WIDTH, (index / _map->activeMap->mapWidth) * TILE_HEIGHT),
            sf::Vector2f(TILE_WIDTH, TILE_HEIGHT)
        );
    }
    sf::FloatRect MapCollider::getBoundsOfTile(int x, int y) {
        return getBoundsOfTile((y * _map->activeMap->mapWidth) + x);
    }

    int MapCollider::clip(int n, int lower, int upper) {
        return std::max(lower, std::min(n, upper));
    }

    bool MapCollider::isSolid(int INDEX) {
        return _map->_mapInfo->tiles[_map->getTileID(INDEX)]->hasFlag(Tile::TileFlags::solid);
    }


    bool MapCollider::isLeftToAngleRange(float ANGLE) {
        return ANGLE > -180.0f + MAPCOLLIDER_ANGLERANGEBOUND && ANGLE <= 0.0f;
    }
    bool MapCollider::isRightToAngleRange(float ANGLE) {
        return ANGLE < 180.0f - MAPCOLLIDER_ANGLERANGEBOUND && ANGLE >= 0.0f;
    }
    bool MapCollider::isInAngleRange(float ANGLE) {
        return !isLeftToAngleRange(ANGLE) && !isRightToAngleRange(ANGLE);
    }

    void MapCollider::drawToGameDataRenderWindow() {
        for(CollisionComponent* col : groundedCheckThisUpdate)
            _map->_data->renderWindow->draw(col->lines, (sizeof(col->lines) / sizeof(*col->lines)), sf::Lines);
    }
}