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

using namespace Framework;

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

Object2D::~Object2D()
{}

// Übergibt einen Void Funktionspointer auf eine Aktion die einmalig vom Hauptthread ausgeführt werden soll. (Passiert nach dem 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::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;
}

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

bool Object2D::handleCollision( Object2D *obj )
{
    Vertex hp;
    if( istModelInnen( obj, &hp ) ) // hp wird auf den aufprallpunkt gesetzt
    {
        // Geschwindigkeit von diesem objekt mit rotation
        Vertex v1 = getSpeed() + getWorldDir( getObjectPos( hp ).rotation( rSpeed ) - getObjectPos( hp ) );
        // Geschwindigkeit des anderen Objektes mit rotation
        Vertex v2 = obj->getSpeed() + getWorldDir( obj->getObjectPos( hp ).rotation( obj->getDrehungSpeed() ) - obj->getObjectPos( hp ) );
        if( ( hp - obj->getPosition() ).getLengthSq() > ( hp + v1 * 0.03f - obj->getPosition() ).getLengthSq() ||
            ( hp - getPosition() ).getLengthSq() > ( hp + v2 * 0.03f - getPosition() ).getLengthSq() )
        { // nur wenn sie sich aufeinander zu bewegen
            float m1 = getMasse() * v1.getLength(); // fläche von Objekt 1
            float m2 = obj->getMasse() * v2.getLength(); // fläche von Objekt 2
            if( m1 == 0 || m2 == 0 )
                return 0; // falls ein objekt keine masse hat ignoriere die kollision
            float nm1 = m1 / ( m1 + m2 ); // koeffizient für objekt 2
            float nm2 = m2 / ( m1 + m2 ); // koeffizient für Objekt 1
            //rSpeed *= nm1; // Drehgeschwindigkeit anpassen (objekt 1)
            //speed *= nm1; // Bewegungsgeschwindigkeit anpassen (objekt 1)
            //obj->setDrehungSpeed( obj->getDrehungSpeed() * nm2 ); // Drehgeschwindigkeit anpassen (objekt 2)
            //obj->setSpeed( obj->getSpeed() * nm2 ); // Bewegungsgeschwindigkeit anpassen (objekt 2)
            float speedSumLength = getSpeed().getLength() + obj->getSpeed().getLength();
            rSpeed = 0;
            speed = Vertex();
            obj->setDrehungSpeed( 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 WeltInfo &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 * 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 > (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::istPunktInnen( Vertex p, bool ignoreTransparent ) const
{
    return 0;
}

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

bool Object2D::istModelInnen( 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::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;
}

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


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

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;
}

Object2D *Welt2D::zObjectAt( int x, int y, bool ignoreTransparentFlag )
{
    for( auto o = objects->getIterator(); o; o++ )
    {
        if( o->istPunktInnen( Punkt( x, y ), ignoreTransparentFlag ) )
            return o._;
    }
    return 0;
}

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

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

void Welt2D::removeObject( Object2D *zObj )
{
    int anz = objects->getEintragAnzahl();
    for( int i = 0; i < anz; i++ )
    {
        if( objects->z( i ) == zObj )
        {
            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->getIterator(); obj; 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->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 Welt2D::impuls( Vertex worldPos, Vertex worldDir )
{
    Vertex hitPoint;
    float dist = INFINITY;
    Object2D *o = 0;
    for( auto obj = objects->getIterator(); obj; obj++ )
    {
        if( obj->calcHitPoint( worldPos, worldDir, hitPoint ) )
        {
            if( ( hitPoint - worldPos ).getLengthSq() < dist )
            {
                dist = ( hitPoint - worldPos ).getLengthSq();
                o = obj;
            }
        }
    }
    if( o )
        o->impuls( worldPos, worldDir );
}

bool Welt2D::tick( double zeit )
{
    bool ret = 0;
    for( auto obj = objects->getIterator(); 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 Welt2D::render( Mat3< float > &kamMat, Punkt size, Bild &zRObj, int xOffset, int yOffset, const char *kamName )
{
    for( auto obj = objects->getIterator(); obj; obj++ )
    {
        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 Welt2D::render( Mat3< float > &kamMat, Punkt size, Bild &zRObj, const char *kamName )
{
    if( !info.hasSize || !info.circular )
    {
        for( auto obj = objects->getIterator(); obj; obj++ )
        {
            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 WeltInfo &Welt2D::getWorldInfo() const
{
    return info;
}

Iterator< Object2D * > Welt2D::getMembers()
{
    return objects->getIterator();
}