#include "World2D.h"

#include "Image.h"

using namespace Framework;

Object2D::Object2D()
    : ReferenceCounter()
{
    rSpeed = 0;
    rotation = 0;
    size = 1;
    collision = 1;
}

Object2D::~Object2D() {}

// Passes a void function pointer to an action that is executed once by the
// main thread. (Happens after the tick)
void Object2D::postAction(std::function<void()> action)
{
    actions.push(action);
}

void Object2D::explosion(Vertex worldPos, float intensity)
{
    intensity /= (position - worldPos).getLengthSq();
    speed += (position - worldPos) * intensity;
}

void Object2D::impuls(Vertex start, Vertex speed, float strength) {}

void Object2D::setSpeed(Vertex speed)
{
    this->speed = speed;
}

void Object2D::setSpeed(float x, float y)
{
    speed.x = x, speed.y = y;
}

void Object2D::setPosition(Vertex pos)
{
    position = pos;
}

void Object2D::setPosition(float x, float y)
{
    position = Vertex(x, y);
}

void Object2D::setRotationSpeed(float ds)
{
    rSpeed = ds;
}

void Object2D::setRotation(float rotation)
{
    rotation = rotation;
    while (rotation > PI * 2)
        rotation -= (float)PI * 2;
    while (rotation < 0)
        rotation += (float)PI * 2;
}

void Object2D::addRotation(float rotation)
{
    rotation += rotation;
    while (rotation > PI * 2)
        rotation -= (float)PI * 2;
    while (rotation < 0)
        rotation += (float)PI * 2;
}

void Object2D::setSize(float size)
{
    this->size = size;
}

void Object2D::addSize(float size)
{
    this->size += size;
}

void Object2D::setCollision(bool handle)
{
    collision = handle;
}

bool Object2D::handleCollision(Object2D* obj)
{
    Vertex hp;
    if (isModelInside(obj, &hp)) // hp is set to the collision point
    {
        // Velocity of this object with rotation
        Vertex v1
            = getSpeed()
            + getWorldDir(getObjectPos(hp).rotation(rSpeed) - getObjectPos(hp));
        // Velocity of the other object with rotation
        Vertex v2 = obj->getSpeed()
                  + getWorldDir(
                      obj->getObjectPos(hp).rotation(obj->getRotationSpeed())
                      - obj->getObjectPos(hp));
        if ((hp - obj->getPosition()).getLengthSq()
                > (hp + v1 * 0.03f - obj->getPosition()).getLengthSq()
            || (hp - getPosition()).getLengthSq()
                   > (hp + v2 * 0.03f - getPosition()).getLengthSq())
        { // only if they are moving towards each other
            float m1 = getMass() * v1.getLength();      // area of object 1
            float m2 = obj->getMass() * v2.getLength(); // area of object 2
            if (m1 == 0 || m2 == 0)
                return 0;               // if an object has no mass, ignore the
                                        // collision
            float nm1 = m1 / (m1 + m2); // coefficient for object 2
            float nm2 = m2 / (m1 + m2); // coefficient for object 1
            // rSpeed *= nm1; // adjust rotation speed (object 1)
            // speed *= nm1; // adjust movement speed (object 1)
            // obj->setRotationSpeed( obj->getRotationSpeed() * nm2 ); //
            // adjust rotation speed (object 2) obj->setSpeed(
            // obj->getSpeed() * nm2 ); // adjust movement speed
            // (object 2)
            float speedSumLength
                = getSpeed().getLength() + obj->getSpeed().getLength();
            rSpeed = 0;
            speed = Vertex();
            obj->setRotationSpeed(0);
            obj->setSpeed(Vertex());
            if (v2.x || v2.y) impuls(hp - v2, v2);
            if (getSpeed().getLength() > 0)
                setSpeed(getSpeed().normalize() * speedSumLength * nm2);
            if (v1.x || v1.y) obj->impuls(hp - v1, v1);
            if (obj->getSpeed().getLength() > 0)
                obj->setSpeed(
                    obj->getSpeed().normalize() * speedSumLength * nm1);
            return 1;
        }
    }
    return 0;
}

bool Object2D::tick(const WorldInfo& info, double zeit)
{
    while (!actions.empty())
    {
        actions.front()();
        actions.pop();
    }
    rotation += rSpeed * (float)zeit;
    while (rotation > PI * 2)
        rotation -= (float)PI * 2;
    while (rotation < 0)
        rotation += (float)PI * 2;
    position += speed * (float)zeit;
    while (zeit > 1)
    {
        rSpeed -= rSpeed
                - (rSpeed / (1 + info.airResistance * getAirResistance()));
        speed
            -= speed - (speed / (1 + info.airResistance * getAirResistance()));
        zeit -= 1;
    }
    rSpeed
        -= (rSpeed - (rSpeed / (1 + info.airResistance * getAirResistance())))
         * (float)zeit;
    speed -= (speed - (speed / (1 + info.airResistance * getAirResistance())))
           * (float)zeit;
    if (info.circular && info.hasSize && info.size.x && info.size.y)
    {
        while (position.x > (float)info.size.x)
            position.x -= (float)info.size.x;
        while (position.x < 0)
            position.x += (float)info.size.x;
        while (position.y > (float)info.size.y)
            position.y -= (float)info.size.y;
        while (position.y < 0)
            position.y += (float)info.size.y;
    }
    return rSpeed != 0 || speed != Vertex(0, 0);
}

bool Object2D::isPointInside(Vertex p, bool ignoreTransparent) const
{
    return 0;
}

bool Object2D::isLineInside(Vertex a, Vertex b, bool ignoreTransparent) const
{
    return 0;
}

bool Object2D::isModelInside(
    const Object2D* zObj, Vertex* sp, bool end, bool ignoreTransparent) const
{
    return 0;
}

Mat3<float> Object2D::getObjectMatrix() const
{
    return Mat3<float>::translation(position) * Mat3<float>::rotation(rotation)
         * Mat3<float>::scaling(size);
}

Mat3<float> Object2D::getInverseObjectMatrix() const
{
    return Mat3<float>::scaling(1 / size) * Mat3<float>::rotation(-rotation)
         * Mat3<float>::translation(-position);
}

Vertex Object2D::getObjectPos(Vertex worldPos) const
{
    return (worldPos - position).rotation(-rotation) * (1 / size);
}

Vertex Object2D::getObjectDir(Vertex worldDir) const
{
    return Vertex(worldDir.x, worldDir.y).rotation(-rotation) * (1 / size);
}

Vertex Object2D::getWorldPos(Vertex objectPos) const
{
    return (Vertex(objectPos) * size).rotation(rotation) + position;
}

Vertex Object2D::getWorldDir(Vertex objectDir) const
{
    return (Vertex(objectDir) * size).rotation(rotation);
}

Vertex Object2D::getSpeed() const
{
    return speed;
}

Vertex Object2D::getPosition() const
{
    return position;
}

float Object2D::getRotationSpeed() const
{
    return rSpeed;
}

float Object2D::getRotation() const
{
    return rotation;
}

float Object2D::getSize() const
{
    return size;
}

bool Object2D::calcHitPoint(Vertex pos, Vertex dir, Vertex& hitpoint) const
{
    return 0;
}

float Object2D::getAirResistance() const
{
    return 0;
}

float Object2D::getMass() const
{
    return 0;
}

bool Object2D::canCollide()
{
    return collision;
}

World2D::World2D()
    : ReferenceCounter()
{
    objects = new RCArray<Object2D>();
    memset((void*)&info, 0, sizeof(WorldInfo));
}

World2D::~World2D()
{
    objects->release();
}

void World2D::setAirResistance(float resistance)
{
    info.airResistance = resistance;
}

void World2D::setSize(int width, int height)
{
    info.size.x = width;
    info.size.y = height;
}

void World2D::setSize(bool hasSize)
{
    info.hasSize = hasSize;
}

void World2D::setCircular(bool circular)
{
    info.circular = circular;
}

Object2D* World2D::zObjectAt(int x, int y, bool ignoreTransparentFlag)
{
    for (auto o : *objects)
    {
        if (o->isPointInside(Point(x, y), ignoreTransparentFlag)) return o;
    }
    return 0;
}

Object2D* World2D::getObjectAt(int x, int y, bool ignoreTransparentFlag)
{
    Object2D* tmp = zObjectAt(x, y, ignoreTransparentFlag);
    return tmp ? dynamic_cast<Object2D*>(tmp->getThis()) : 0;
}

void World2D::addObject(Object2D* obj)
{
    objects->add(obj);
}

void World2D::removeObject(Object2D* zObj)
{
    int anz = objects->getEntryCount();
    for (int i = 0; i < anz; i++)
    {
        if (objects->z(i) == zObj)
        {
            objects->remove(i);
            i--;
        }
    }
}

void World2D::removeAll()
{
    objects->clear();
}

void World2D::explosion(Vertex worldPos, float intensity, float maxRad)
{
    maxRad = maxRad * maxRad;
    for (auto obj : *objects)
    {
        if (info.circular && info.hasSize && info.size.x && info.size.y)
        {
            Vertex offsets[] = {Vertex(0, 0),
                Vertex((float)info.size.x, 0),
                Vertex(0, (float)info.size.y),
                Vertex((float)info.size.x, (float)info.size.y),
                Vertex((float)-info.size.x, 0),
                Vertex(0, (float)-info.size.y),
                Vertex((float)-info.size.x, (float)-info.size.y),
                Vertex((float)-info.size.x, (float)info.size.y),
                Vertex((float)info.size.x, (float)-info.size.y)};
            Vertex offset;
            float minDist = INFINITY;
            for (Vertex p : offsets)
            {
                float dist
                    = (obj->getPosition() - (worldPos - p)).getLengthSq();
                if (dist < minDist)
                {
                    minDist = dist;
                    offset = p;
                }
            }
            if ((obj->getPosition() - (worldPos - offset)).getLengthSq()
                < maxRad)
                obj->explosion(worldPos - offset, intensity);
        }
        else if ((obj->getPosition() - worldPos).getLengthSq() < maxRad)
            obj->explosion(worldPos, intensity);
    }
}

void World2D::impuls(Vertex worldPos, Vertex worldDir)
{
    Vertex hitPoint;
    float dist = INFINITY;
    Object2D* o = 0;
    for (auto obj : *objects)
    {
        if (obj->calcHitPoint(worldPos, worldDir, hitPoint))
        {
            if ((hitPoint - worldPos).getLengthSq() < dist)
            {
                dist = (hitPoint - worldPos).getLengthSq();
                o = obj;
            }
        }
    }
    if (o) o->impuls(worldPos, worldDir);
}

bool World2D::tick(double zeit)
{
    bool ret = 0;
    for (auto obj = objects->begin(); obj; obj++)
    {
        if (obj.hasNext() && obj->canCollide())
        {
            for (auto obj2 = obj.next(); obj2; obj2++)
            {
                if (obj2->canCollide()) obj->handleCollision(obj2);
            }
        }
        ret |= obj->tick(info, zeit);
    }
    return ret;
}

void World2D::render(Mat3<float>& kamMat,
    Point size,
    Image& zRObj,
    int xOffset,
    int yOffset,
    const char* kamName)
{
    for (auto obj : *objects)
    {
        Rect2<float> bnd = obj->getBoundingBox();
        Vertex topRight = Vertex(bnd.bottomRight.x, bnd.topLeft.y);
        Vertex bottomLeft = Vertex(bnd.topLeft.x, bnd.bottomRight.y);
        Mat3<float> km
            = kamMat
            * Mat3<float>::translation(Vertex((float)xOffset, (float)yOffset));
        bnd.bottomRight = km * bnd.bottomRight;
        bnd.topLeft = km * bnd.topLeft;
        topRight = km * topRight;
        bottomLeft = km * bottomLeft;
        if ((bnd.bottomRight.x >= 0 && bnd.bottomRight.x < (float)size.x)
            || (bnd.bottomRight.y >= 0 && bnd.bottomRight.y < (float)size.y)
            || (bnd.topLeft.x >= 0 && bnd.topLeft.x < (float)size.x)
            || (bnd.topLeft.y >= 0 && bnd.topLeft.y < (float)size.y)
            || (topRight.x >= 0 && topRight.x < (float)size.x)
            || (topRight.y >= 0 && topRight.y < (float)size.y)
            || (bottomLeft.x >= 0 && bottomLeft.x < (float)size.x)
            || (bottomLeft.y >= 0 && bottomLeft.y < (float)size.y))
            obj->render(km, zRObj, kamName);
    }
}

void World2D::render(
    Mat3<float>& kamMat, Point size, Image& zRObj, const char* kamName)
{
    if (!info.hasSize || !info.circular)
    {
        for (auto obj : *objects)
        {
            Rect2<float> bnd = obj->getBoundingBox();
            Vertex topRight = Vertex(bnd.topLeft.y, bnd.bottomRight.x);
            Vertex bottomLeft = Vertex(bnd.topLeft.x, bnd.bottomRight.y);
            bnd.bottomRight = kamMat * bnd.bottomRight;
            bnd.topLeft = kamMat * bnd.topLeft;
            topRight = kamMat * topRight;
            bottomLeft = kamMat * bottomLeft;
            if ((bnd.bottomRight.x >= 0 && bnd.bottomRight.x < (float)size.x)
                || (bnd.bottomRight.y >= 0 && bnd.bottomRight.y < (float)size.y)
                || (bnd.topLeft.x >= 0 && bnd.topLeft.x < (float)size.x)
                || (bnd.topLeft.y >= 0 && bnd.topLeft.y < (float)size.y)
                || (topRight.x >= 0 && topRight.x < (float)size.x)
                || (topRight.y >= 0 && topRight.y < (float)size.y)
                || (bottomLeft.x >= 0 && bottomLeft.x < (float)size.x)
                || (bottomLeft.y >= 0 && bottomLeft.y < (float)size.y))
                obj->render(kamMat, zRObj, kamName);
        }
    }
    else if (info.circular)
    {
        render(kamMat, size, zRObj, 0, 0, kamName);
        render(kamMat, size, zRObj, info.size.x, 0, kamName);
        render(kamMat, size, zRObj, 0, info.size.y, kamName);
        render(kamMat, size, zRObj, info.size.x, info.size.y, kamName);
        render(kamMat, size, zRObj, -info.size.x, 0, kamName);
        render(kamMat, size, zRObj, 0, -info.size.y, kamName);
        render(kamMat, size, zRObj, -info.size.x, -info.size.y, kamName);
        render(kamMat, size, zRObj, -info.size.x, +info.size.y, kamName);
        render(kamMat, size, zRObj, +info.size.x, -info.size.y, kamName);
    }
}

const WorldInfo& World2D::getWorldInfo() const
{
    return info;
}

ArrayIterator<Object2D*> World2D::getMembers()
{
    return objects->begin();
}