import java.io.*;
import java.net.*;

public class Controller extends Thread {

    Space space = Space.getInstance();
    private Scene scene;

    public Controller (Scene scene) {
        this.scene = scene;
        start();
    }

    public void run () {
        float h = (float) (Math.PI/2-0.3); // heading
        double dh = 0.0872222; //5 stupnov
        double sp = 0.05; // speed
        boolean bump = false;
        boolean prvy = true;
        scene.setPos(scene.getPosX(),scene.getPosY(),h);
        for (;;) {
            // aktuatory
            int turn = ((Integer) space.read("Turn",new Integer(0))).intValue();
            float forward = ((Float) space.read("Forward",new Float(0f))).floatValue();
            h += turn*dh;
            if (h>=2*Math.PI) h -= 2*Math.PI;
            if (h<0.0) h += 2*Math.PI;
            float x = scene.getPosX();
            float y = scene.getPosY();
            System.out.println("turn="+turn+"->"+h+" , forward:"+forward+"->"+x+","+y);
            float fx = x, fy = y;
            bump = false;
            if (forward != 0 || turn != 0) {
                x += (float) forward*sp*Math.cos(h);
                y += (float) forward*sp*Math.sin(h);
                scene.setPos(x,y,h);
                if (scene.collide()) {
                    bump = true;
                    x=fx;
                    y=fy;
                    scene.setPos(x,y,h);
                }
            }
            space.write("Compass",new Float((float)h));
            space.write("Bump",new Boolean(bump));
            double[] d = scene.measure(h);
            space.write("Sonar",d);
            System.out.print("x="+Math.round(x*100.0)/100.0+" y="+Math.round(y*100.0)/100.0+" h="+Math.round(h*100.0)/100.0+" b="+bump+" d");
            for (int i=0; i<12; i++) System.out.print(""+i+"="+d[i]+" ");
            System.out.println();
            if (prvy) {
                Starter.initialize();
                prvy = false;
            }
            try {
                Thread.sleep(100);
            } catch (InterruptedException ee) {
            }
        }
    }

}
