package com.inari.firefly.platformer;

import com.inari.commons.geom.BitMask;
import com.inari.commons.geom.Rectangle;
import com.inari.firefly.entity.EEntity;
import com.inari.firefly.entity.ETransform;
import com.inari.firefly.physics.collision.CollisionResolver;
import com.inari.firefly.physics.collision.ContactScan;
import com.inari.firefly.physics.collision.ECollision;
import com.inari.firefly.physics.movement.EMovement;

public final class PFCollisionResolver extends CollisionResolver {
    
    private Rectangle vScan = new Rectangle( 0, 0, 1, 5 );
    private Rectangle hScan = new Rectangle( 2, 0, 4, 1 );

    protected PFCollisionResolver( int id ) {
        super( id );
    }

    @Override
    public final void resolve( final int entityId ) {
        final EEntity entity = context.getEntityComponent( entityId, EEntity.TYPE_KEY );
        final ECollision collision = context.getEntityComponent( entityId, ECollision.TYPE_KEY );
        final ETransform transform = context.getEntityComponent( entityId, ETransform.TYPE_KEY );
        final EMovement movement = context.getEntityComponent( entityId, EMovement.TYPE_KEY );
        
        final ContactScan contactScan = collision.getContactScan();
        final BitMask intersectionMask = contactScan.getIntersectionMask();
        final boolean groundContact = entity.hasAspect( PFState.ON_GROUND );

        //System.out.println( "contactScan: "+contactScan.getIntersectionMask() );

        entity.resetAspect( PFState.ON_GROUND );
        boolean onGround = onGround( intersectionMask );
        
        boolean gotFromLadder = false;
        if ( !contactScan.hasContact( PFContact.LADDER ) && entity.hasAspect( PFState.ON_LADDER ) ) {
            entity.resetAspect( PFState.ON_LADDER );
            gotFromLadder = true;
        }
        if ( ( groundContact || gotFromLadder ) && onGround ) {
            entity.setAspect( PFState.ON_GROUND );
        }
        
        if ( intersectionMask.isEmpty() ) {
            return;
        }
        
        final float velocityY = movement.getVelocityY();
        final float velocityX = movement.getVelocityX();
        
        int ycorrection = 0;
        if ( velocityY >= 0 ) {
            ycorrection = adjustToGround( intersectionMask );
            if ( ycorrection != 0 && !( ycorrection > 0 && !groundContact ) ) {
                movement.setVelocityY( 0f );
                entity.setAspect( PFState.ON_GROUND );
            }
        } else if ( velocityY < 0 ) {
            hScan.y = 0;
            while( intersectionMask.hasIntersection( hScan ) ) {
                ycorrection++;
                hScan.y++;
            }
        }
        
        if ( ycorrection != 0 ) {
            if ( ycorrection < 0 ) { 
                transform.setYpos( (float) Math.ceil( transform.getYpos() ) + ycorrection );
            } else {
                transform.setYpos( (float) Math.floor( transform.getYpos() ) + ycorrection );
            }
        }

        int xcorrection = 0;
        if ( velocityX > 0 ) {
            vScan.x = 7;
            vScan.y = ycorrection;
            while( intersectionMask.hasIntersection( vScan ) ) {
                xcorrection++;
                vScan.x--;
            }
            transform.setXpos( (float) Math.ceil( transform.getXpos() ) - xcorrection );
        } else if ( velocityX < 0 ) {
            vScan.x = 0;
            vScan.y = ycorrection;
            while( intersectionMask.hasIntersection( vScan ) ) {
                xcorrection++;
                vScan.x++;
            }
            transform.setXpos( (float) Math.floor( transform.getXpos() ) + xcorrection );
        }
        
         
    }
    
    private boolean onGround( final BitMask intersectionMask ) {
        for ( int i = 1; i < 6; i++ ) {
            if ( intersectionMask.getBit( i, 8 ) ) {
                return true;
            }
        }
        
        return false;
    }
    
    private int adjustToGround( final BitMask intersectionMask ) {
        int correction = 0;
        final int halfHeight = 5;
        for ( int i = 0; i < 10; i++ ) {
            if ( intersectionMask.getBit( 3, 3 + i ) || intersectionMask.getBit( 4, 3 + i ) ) {
                correction = -( halfHeight - i );
                break;
            }
        }
        
        return correction;
    }
 
}

@@ -112,16 +112,58 @@
 tangle;%0A
+import com.inari.firefly.FFInitException;%0A
 import c
@@ -195,16 +195,16 @@
 Entity;%0A
-
 import c
@@ -301,16 +301,76 @@
 solver;%0A
+import com.inari.firefly.physics.collision.CollisionSystem;%0A
 import c
@@ -594,24 +594,74 @@
 lver %7B%0A    %0A
+    private CollisionSystem collisionSystem;%0A    %0A
     private 
@@ -836,24 +836,214 @@
 id );%0A    %7D%0A
+    %0A    %0A%0A    @Override%0A    protected void init() throws FFInitException %7B%0A        super.init();%0A        %0A        collisionSystem = context.getSystem( CollisionSystem.SYSTEM_KEY );%0A    %7D%0A%0A%0A
 %0A    @Overri
@@ -2694,53 +2694,8 @@
 ) %7B%0A
-                movement.setVelocityY( 0f );%0A
     
@@ -2737,32 +2737,86 @@
 te.ON_GROUND );%0A
+            %7D else %7B%0A                ycorrection = 0;%0A
             %7D%0A  
@@ -3318,32 +3318,101 @@
 ;%0A            %7D%0A
+            %0A            collisionSystem.updateContacts( entityId );%0A
         %7D%0A%0A     
@@ -3495,43 +3495,8 @@
  7;%0A
-            vScan.y = ycorrection;%0A
     
@@ -3632,32 +3632,87 @@
   %7D%0A            
+%0A            if ( xcorrection != 0 ) %7B%0A                
 transform.setXpo
@@ -3764,32 +3764,46 @@
  xcorrection );%0A
+            %7D%0A
         %7D else i
@@ -3853,43 +3853,8 @@
  0;%0A
-            vScan.y = ycorrection;%0A
     
@@ -3990,32 +3990,87 @@
   %7D%0A            
+%0A            if ( xcorrection != 0 ) %7B%0A                
 transform.setXpo
@@ -4131,32 +4131,36 @@
 tion );%0A        
+    
 %7D%0A        %0A     
@@ -4153,26 +4153,17 @@
         
-%0A         
+%7D
 %0A    %7D%0A 

fix collision resolving