#include "Welt2D.h"
#include "Bild.h"

using namespace Framework;

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

Object2D::~Object2D()
{}

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

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

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::setDrehungSpeed( float ds )
{
    rSpeed = ds;
}

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

void Object2D::addDrehung( float drehung )
{
    rotation += drehung;
    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;
}

bool Object2D::handleCollision( Object2D *obj )
{
    Vertex hp;
    if( istModelInnen( obj, &hp ) )
    {
        Vertex v1 = getSpeed() + getWorldDir( getObjectPos( hp ).rotation( rSpeed ) - getObjectPos( hp ) );
        Vertex v2 = obj->getSpeed() + getWorldDir( obj->getObjectPos( hp ).rotation( obj->getDrehungSpeed() ) - obj->getObjectPos( hp ) );
        if( ( position - obj->getPosition() ).getLengthSq() > ( position + v1 * 0.03f - obj->getPosition() - v2 * 0.03f ).getLengthSq() )
        {
            float m1 = getMasse();
            float m2 = obj->getMasse();
            if( m1 == 0 || m2 == 0 )
                return 0;
            rSpeed = 0;
            speed = Vertex( 0, 0 );
            obj->setDrehungSpeed( 0 );
            obj->setSpeed( 0, 0 );
            if( v2.x || v2.y )
                impuls( hp - v2, v2 * ( m2 / ( m1 + m2 ) ) );
            if( v1.x || v1.y )
                obj->impuls( hp - v1, v1 * ( m1 / ( m1 + m2 ) ) );
            return 1;
        }
    }
    return 0;
}

bool Object2D::tick( const WeltInfo &info, double zeit )
{
    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 * getLuftWiederstand() ) );
        speed -= speed - ( speed / ( 1 + info.airResistance * getLuftWiederstand() ) );
        zeit -= 1;
    }
    rSpeed -= ( rSpeed - ( rSpeed / ( 1 + info.airResistance * getLuftWiederstand() ) ) ) * (float)zeit;
    speed -= ( speed - ( speed / ( 1 + info.airResistance * getLuftWiederstand() ) ) ) * (float)zeit;
    if( info.circular && info.hasSize && info.size.x && info.size.y )
    {
        while( position.x > info.size.x )
            position.x -= (float)info.size.x;
        while( position.x < 0 )
            position.x += (float)info.size.x;
        while( position.y > 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::istPunktInnen( Vertex p ) const
{
    return 0;
}

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

bool Object2D::istModelInnen( const Object2D *zObj, Vertex *sp, bool end ) 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::getDrehungSpeed() const
{
    return rSpeed;
}

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

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

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

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

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

Object2D *Object2D::getThis()
{
    ref++;
    return this;
}

Object2D *Object2D::release()
{
    if( !--ref )
        delete this;
    return 0;
}


Welt2D::Welt2D()
{
    objects = new RCArray< Object2D >();
    memset( &info, 0, sizeof( WeltInfo ) );
    ref = 1;
}

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

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

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

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

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

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

void Welt2D::removeObject( Object2D *obj )
{
    int anz = objects->getEintragAnzahl();
    for( int i = 0; i < anz; i++ )
    {
        if( objects->get( i ) == obj )
        {
            objects->remove( i );
            i--;
        }
    }
}

void Welt2D::removeAll()
{
    objects->leeren();
}

void Welt2D::explosion( Vertex worldPos, float intensity, float maxRad )
{
    maxRad = maxRad * maxRad;
    for( auto obj = objects->getArray(); obj.set; obj++ )
    {
        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.var->getPosition() - (worldPos - p) ).getLengthSq();
                if( dist < minDist )
                {
                    minDist = dist;
                    offset = p;
                }
            }
            if( ( obj.var->getPosition() - (worldPos - offset) ).getLengthSq() < maxRad )
                obj.var->explosion( worldPos - offset, intensity );
        }
        else if( ( obj.var->getPosition() - worldPos ).getLengthSq() < maxRad )
            obj.var->explosion( worldPos, intensity );
    }
}

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

bool Welt2D::tick( double zeit )
{
    bool ret = 0;
    for( auto obj = objects->getArray(); obj.set; obj++ )
    {
        if( obj.next )
        {
            for( auto obj2 = *obj.next; obj2.set; obj2++ )
                obj.var->handleCollision( obj2 );
        }
        ret |= obj.var->tick( info, zeit );
    }
    return ret;
}

void Welt2D::render( Mat3< float > &kamMat, Punkt size, Bild &zRObj, int xOffset, int yOffset )
{
    for( auto obj = objects->getArray(); obj.set; obj++ )
    {
        Rect2< float > bnd = obj.var->getBoundingBox();
        Vertex topRight = Vertex( bnd.topLeft.y, bnd.bottomRight.x );
        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 < size.x ) ||
            ( bnd.bottomRight.y >= 0 && bnd.bottomRight.y < size.y ) ||
            ( bnd.topLeft.x >= 0 && bnd.topLeft.x < size.x ) ||
            ( bnd.topLeft.y >= 0 && bnd.topLeft.y < size.y ) ||
            ( topRight.x >= 0 && topRight.x < size.x ) ||
            ( topRight.y >= 0 && topRight.y < size.y ) ||
            ( bottomLeft.x >= 0 && bottomLeft.x < size.x ) ||
            ( bottomLeft.y >= 0 && bottomLeft.y < size.y ) )
            obj.var->render( km, zRObj );
    }
}

void Welt2D::render( Mat3< float > &kamMat, Punkt size, Bild &zRObj )
{
    if( !info.hasSize || !info.circular )
    {
        for( auto obj = objects->getArray(); obj.set; obj++ )
        {
            Rect2< float > bnd = obj.var->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 < size.x ) ||
                ( bnd.bottomRight.y >= 0 && bnd.bottomRight.y < size.y ) ||
                ( bnd.topLeft.x >= 0 && bnd.topLeft.x < size.x ) ||
                ( bnd.topLeft.y >= 0 && bnd.topLeft.y < size.y ) ||
                ( topRight.x >= 0 && topRight.x < size.x ) ||
                ( topRight.y >= 0 && topRight.y < size.y ) ||
                ( bottomLeft.x >= 0 && bottomLeft.x < size.x ) ||
                ( bottomLeft.y >= 0 && bottomLeft.y < size.y ) )
                obj.var->render( kamMat, zRObj );
        }
    }
    else if( info.circular )
    {
        render( kamMat, size, zRObj, 0, 0 );
        render( kamMat, size, zRObj, info.size.x, 0 );
        render( kamMat, size, zRObj, 0, info.size.y );
        render( kamMat, size, zRObj, info.size.x, info.size.y );
        render( kamMat, size, zRObj, -info.size.x, 0 );
        render( kamMat, size, zRObj, 0, -info.size.y );
        render( kamMat, size, zRObj, -info.size.x, -info.size.y );
        render( kamMat, size, zRObj, -info.size.x, +info.size.y );
        render( kamMat, size, zRObj, +info.size.x, -info.size.y );
    }
}

const WeltInfo &Welt2D::getWorldInfo() const
{
    return info;
}

Welt2D *Welt2D::getThis()
{
    ref++;
    return this;
}

Welt2D *Welt2D::release()
{
    if( !--ref )
        delete this;
    return 0;
}