package org.wkh.swarmscale.physicalexample;

import java.awt.Color;
import org.dyn4j.dynamics.BodyFixture;
import org.dyn4j.geometry.Circle;
import org.dyn4j.geometry.MassType;
import org.dyn4j.geometry.Rectangle;
import org.dyn4j.geometry.Vector2;

/**
 * A simple physical system where a circle at the origin falls downward under the influence of gravity.
 */
public class BallGravitySystem extends AbstractPhysicalSystem {

    private RenderedBody circle;
    private RenderedBody floor;


    private double previousPosition;
    private double currentPosition;
    private double startingPosition;

    @Override
    public void initializeWorld() {
        super.initializeWorld();
        
        // create the floor
        Rectangle floorRect = new Rectangle(15.0, 1.0);
        floor = new RenderedBody(Color.DARK_GRAY);
        floor.addFixture(new BodyFixture(floorRect));
        floor.setMass(MassType.INFINITE);
        // move the floor down a bit
        floor.translate(0.0, -4.0);
        world.addBody(floor);

        // create a circle
        Circle cirShape = new Circle(0.5);
        circle = new RenderedBody(Color.BLACK);
        circle.addFixture(cirShape);
        circle.setMass(MassType.NORMAL);

        circle.setLinearDamping(0.05);
        world.addBody(circle);
    }

    @Override
    protected void beforeSimulationLoopStart() {
        previousPosition = circle.getTransform().getTranslationY();
        startingPosition = previousPosition;
    }

    @Override
    protected void postSimulationStep() {
        currentPosition = circle.getTransform().getTranslationY();
        double positionDelta = currentPosition - startingPosition;

        if (currentPosition != previousPosition) {
            if (positionDelta < 0) {
                /* produces a roughly stable position */
                circle.applyImpulse(new Vector2(0, 0.129));
            }

            System.out.println(getElapsedTime() + " " + positionDelta);
            previousPosition = currentPosition;

            if (circle.isInContact(floor)) {
                stop();
            }
        }
    }

    public static void main(String[] args) {
        PhysicalSystem system = new BallGravitySystem();
        system.initializeWorld();
        /* run for ~1 second */
        system.runSimulationLoop(1000);
    }
}

@@ -1705,9 +1705,9 @@
 ion 
-!
+=
 = pr
@@ -1720,32 +1720,67 @@
 Position) %7B%0A    
+        return;%0A        %7D%0A        %0A
         if (posi
@@ -1808,22 +1808,46 @@
         
-    /*
+/* apply proportional control;
  produce
@@ -1854,15 +1854,16 @@
 s a 
-roughly
+somewhat
  sta
@@ -1875,23 +1875,45 @@
 osition 
-*/%0A    
+albeit an oscillating one */%0A
         
@@ -1957,11 +1957,24 @@
 , 0.
-129
+5*-positionDelta
 ));%0A
@@ -1985,26 +1985,19 @@
     
-    %7D%0A%0A   
+%7D%0A%0A
         
  Sys
@@ -1992,17 +1992,16 @@
         
- 
 System.o
@@ -2056,28 +2056,24 @@
 a);%0A        
-    
 previousPosi
@@ -2101,28 +2101,24 @@
 n;%0A%0A        
- 
-   
 if (circle.i
@@ -2154,17 +2154,55 @@
     
-    stop(
+System.out.println(%22Circle hit floor, stopping%22
 );%0A 
@@ -2204,33 +2204,39 @@
 %22);%0A            
-%7D
+stop();
 %0A        %7D%0A    %7D
@@ -2377,40 +2377,8 @@
 ();%0A
-        /* run for ~1 second */%0A
     
@@ -2410,16 +2410,17 @@
 oop(1000
+0
 );%0A    %7D

make ball system have an actual proportional controller