#include <iostream>
#include <SFML/Graphics.hpp>
#include <cmath>
using namespace std;

sf::Clock g_Clock;
float     g_LastTime = 0.0f;
float     g_Delta = 0.0f;
sf::RenderWindow* window;

float worldHeight = 640.0f;

sf::Vector2f pPos(300, 350);
sf::Vector2f pVel(0,0);

bool world[10][10] = { {1,1,1,1,1,1,1,1,1,1},
                       {1,0,1,0,0,0,0,0,0,1},
                       {1,0,1,0,0,0,0,0,0,1},
                       {1,0,0,0,0,0,0,0,0,1},
                       {1,0,0,0,0,0,0,0,0,1},
                       {1,0,0,0,0,0,0,1,1,1},
                       {1,0,0,0,0,1,1,1,1,1},
                       {1,0,0,0,0,0,0,0,0,1},
                       {1,0,0,0,0,0,0,0,0,1},
                       {1,1,1,1,1,1,1,1,1,1}};

bool blockIsSolid(int x, int y)
{
    x = min(x, 9); x = max(x, 0);
    y = min(y, 9); y = max(y, 0);
    return world[y][x];
}

float length(sf::Vector2f vec)
{
    return sqrt(vec.x*vec.x+vec.y*vec.y);
}

void drawPoint(sf::Vector2f& coord)
{
    sf::RectangleShape As = sf::RectangleShape(sf::Vector2f(4.0f,4.0f));
    As.setOrigin(sf::Vector2f(2.0f,2.0f));
    As.setPosition(coord.x, coord.y);
    window->draw(As);
}

void updatePlayer()
{
    float koef = max(500.0f/length(pVel),1.5f);
    sf::Vector2f delta = pVel*koef*g_Delta;
    sf::Vector2f points[6];
    points[0] = pPos + sf::Vector2f(-28.0f,-58.0f);
    points[1] = pPos + sf::Vector2f(-28.0f,  0.0f);
    points[2] = pPos + sf::Vector2f(-28.0f,+58.0f);
    points[3] = pPos + sf::Vector2f(+28.0f,+58.0f);
    points[4] = pPos + sf::Vector2f(+28.0f,  0.0f);
    points[5] = pPos + sf::Vector2f(+28.0f,-58.0f);

    for(int i=0; i<6; i++)
    {
        if(delta.x>0)
        {
            if(delta.y>0)
            {
                if(i<2)
                    continue;
            } else
            if(delta.y<0)
            {
                if(i<3 && i!=0)
                    continue;
            } else
            {
                if(i<3)
                    continue;
            }
        } else
        if(delta.x<0)
        {
            if(delta.y>0)
            {
                if(i>3)
                    continue;
            } else
            if(delta.y<0)
            {
                if(i>2 && i!=5)
                    continue;
            } else
            {
                if(i>2)
                    continue;
            }
        } else
        {
            if(delta.y>0)
            {
                if(i!=2 && i!=3)
                    continue;
            } else
            if(delta.y<0)
            {
                if(i!=0 && i!=5)
                    continue;
            } else
            {
                return;
            }
        }
        sf::Vector2f &point = points[i];

        int hLoop = ((int)delta.y)/64;
        int vLoop = ((int)delta.x)/64;

        float tanAlpha = (delta.y!=0.0f?delta.x/delta.y:0.0f);

        float dDist = length(delta);
        float sDist = dDist*2.0f;

        //HORIZONTAL INTERSECTION
        if(delta.y!=0.0f)
        {
            sf::Vector2f A;
            A.y = (((int)point.y)/64)*64.0f + (delta.y>0?64.0f:-1.0f);
            A.x = point.x - (point.y - A.y)*tanAlpha;
            sf::Vector2f D;
            D.y = (delta.y>0?64.0f:-64.0f);
            D.x = (delta.y>0?64.0f:-64.0f)*tanAlpha;

            do
            {
                int cX, cY;
                cX = ((int)A.x)/64;
                cY = ((int)A.y)/64;
                if(blockIsSolid(cX, cY))
                {
                    sDist = min(sDist, length(A-point));
                    break;
                }
                A += D;
            } while(hLoop--);
        }

        //VERTICAL INTERSECTION
        if(delta.x!=0.0f)
        {
            sf::Vector2f A;
            A.x = (((int)point.x)/64)*64 + (delta.x>0?64.0f:-1.0f);
            A.y = (tanAlpha!=0.0f?point.y + (A.x - point.x)/tanAlpha:point.y);
            sf::Vector2f D;
            D.x = (delta.x>0?64.0f:-64.0f);
            D.y = (tanAlpha!=0.0f?(delta.x>0?64.0f:-64.0f)/tanAlpha:0.0f);

            do
            {
                int cX, cY;
                cX = ((int)A.x)/64;
                cY = ((int)A.y)/64;
                if(blockIsSolid(cX, cY))
                {
                    sDist = min(sDist, length(A-point));
                    break;
                }
                A += D;
            } while(vLoop--);
        }

        if(sDist < dDist)
        {
            if(pVel.x != 0.0f && pVel.y != 0.0f)
            {
                sf::Vector2f oldVel = pVel;
                pVel.y = 0.0f;
                updatePlayer();
                pVel = oldVel;
                pVel.x = 0.0f;
                updatePlayer();
            }
            return;
        }

    }

    pPos += delta/koef;
    return;
}

void renWorld()
{
    for(int i=9; i>=0; i--)
    {
        for(int j=0; j<10; j++)
        {
            if(world[i][j])
            {
                sf::RectangleShape rect = sf::RectangleShape(sf::Vector2f(64.0f, 64.0f));
                rect.setPosition(j*64, i*64);
                rect.setFillColor(sf::Color(255*(j+1)/10.0, 255*(i+1)/10.0, 0, 128));
                window->draw(rect);
            }
        }
    }
}

void renPlayer()
{
    sf::RectangleShape prect = sf::RectangleShape(sf::Vector2f(64.0f, 128.0f));
    prect.setOrigin(32.0f, 64.0f);
    prect.setPosition(pPos.x, pPos.y);
    prect.setFillColor(sf::Color(255,255,255,128));
    window->draw(prect);
}

int main()
{
    window = new sf::RenderWindow(sf::VideoMode(640, 640), "test", sf::Style::Default & ~sf::Style::Resize);
    window->setActive();
    g_Clock.restart();
    float speed = 100.0f;
    while (window->isOpen())
    {
        sf::Event event;
        while (window->pollEvent(event))
        {
            switch(event.type)
            {
                case sf::Event::Closed:
                    {
                        window->close();
                        break;
                    }
                case sf::Event::KeyReleased:
                    {
                        if(event.key.code == sf::Keyboard::E)
                        {
                            speed += 20.0f;
                        } else
                        if(event.key.code == sf::Keyboard::R)
                        {
                            speed -= 20.0f;
                        }
                    }
                default: break;
            }
        }

        if(sf::Keyboard::isKeyPressed(sf::Keyboard::W))
        {
            pVel.y = -speed;
        } else
        {
            pVel.y = speed;
        }

        if(sf::Keyboard::isKeyPressed(sf::Keyboard::D))
        {
            pVel.x = speed;
        } else
        if(sf::Keyboard::isKeyPressed(sf::Keyboard::A))
        {
            pVel.x = -speed;
        } else
        {
            pVel.x = 0.0f;
        }

        float currTime = g_Clock.getElapsedTime().asSeconds();
        g_Delta = currTime - g_LastTime;
        g_LastTime = currTime;

        window->clear();
        updatePlayer();
        renWorld();
        renPlayer();
        window->display();
        sf::sleep(sf::milliseconds(16));
    }
    delete window;
    window = nullptr;
    return EXIT_SUCCESS;
}