import java.util.*;
import org.jagatoo.input.*;
import org.jagatoo.input.listeners.*;
import org.jagatoo.input.devices.components.*;
import org.jagatoo.input.events.*;
import org.jagatoo.opengl.enums.*;
import org.openmali.vecmath2.*;
import org.xith3d.*;
import org.xith3d.loop.*;
import org.xith3d.loop.opscheduler.*;
import org.xith3d.base.*;
import org.xith3d.render.*;
import org.xith3d.resources.*;
import org.xith3d.scenegraph.*;
import org.xith3d.scenegraph.primitives.*;
import org.xith3d.schedops.movement.*;
import org.xith3d.utility.events.WindowClosingRenderLoopEnder;
import org.xith3d.input.ObjectRotationInputHandler;
import org.xith3d.physics.*;
import org.xith3d.physics.collision.*;
import org.xith3d.physics.joode.JoodePhysicsEngine;
import org.xith3d.physics.simulation.*;

public class Scene extends InputAdapterRenderLoop implements MouseListener {

   	private Space space = Space.getInstance();
    private TransformGroup trans;
    private double[] d = new double[12];
    private double[] r = new double[12];
    private float lastAngle = 0f;

    public void setPos ( float x, float y, float angle ) {
        Transform3D t = new Transform3D(x,0.5f,y);
        t.rotY(-angle);
        lastAngle = angle;
        trans.setTransform(t);
    }

    public void setPos ( float x, float y ) {
        setPos(x,y,lastAngle);
    }

    public float getAngle () {
        return lastAngle;
    }

    public float getPosX () {
        return trans.getTransform().getTranslation().getX();
    }

    public float getPosY () {
        return trans.getTransform().getTranslation().getZ();
    }

    public boolean collide() {
        float l = 2.35f;
        float ll = 2.8f;
        float dv = 0.1f;
        float x = trans.getTransform().getTranslation().getX();
        float y = trans.getTransform().getTranslation().getZ();
        if (x > ll) return false;
        if (x < -ll) return false;
        if (y > ll) return false;
        if (y < -ll) return false;
        if (x < -l) return true;
        if (y < -l) return true;
        if (y > l) return true;
        if (x > l && y > dv) return true;
        if (x > l && y < -dv) return true;
        return false;
    }

    public static double ray (double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) {
        double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
        if (denom == 0.0) return 1000000.0;
        double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3))/denom;
        double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3))/denom;
        if (ua >= 0.0f && ua <= 1.0f && ub >= 0.0f && ub <= 1.0f)
            return ua * Math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
        return 1000000.0;
    }

    public static double min2 (double x1, double x2) {
        if (x1 <= x2) return x1;
        else return x2;
    }

    public static double min7 (double x1, double x2, double x3, double x4, double x5, double x6, double x7) {
        return min2(min2(min2(x1,x2),min2(x3,x4)),min2(min2(x5,x6),x7));
    }

    public double[] measure (double h) {
        float l = 2.35f;
        float lv = l/32;
        float dv = /*0.1f*/ (l/16) * 1.25F;
        float p = lv * 1.5F;
        l += p;
        float x = trans.getTransform().getTranslation().getX();
        float y = trans.getTransform().getTranslation().getZ();
        for (int i=0; i<12; i++) {
            double hh = h + r[i];
            if (hh>=2*Math.PI) hh -= 2*Math.PI;
            double rx = p*Math.cos(hh);
            double ry = p*Math.sin(hh);
            double dx = 10.0*Math.cos(hh);
            double dy = 10.0*Math.sin(hh);
            d[i] = Math.round( 100.0  *min7 (
                                   ray(x+rx,y+ry,x+rx+dx,y+ry+dy,-l,-l,-l,l),
                                   ray(x+rx,y+ry,x+rx+dx,y+ry+dy,l,-l,l,-dv),
                                   ray(x+rx,y+ry,x+rx+dx,y+ry+dy,l,dv,l,l),
                                   ray(x+rx,y+ry,x+rx+dx,y+ry+dy,-l,l,l,l),
                                   ray(x+rx,y+ry,x+rx+dx,y+ry+dy,-l,-l,l,-l),
                                   ray(x+rx,y+ry,x+rx+dx,y+ry+dy,l,dv,l+lv,dv),
                                   ray(x+rx,y+ry,x+rx+dx,y+ry+dy,l,-dv,l+lv,-dv)
                               ) ) / 100.0;
        }
        return d;
    }

    public Scene() throws Exception {
        super( 120f );

        for (int i=0; i<12; i++) {
            r[i] = i * Math.PI / 6;
        }

        Xith3DEnvironment env = new Xith3DEnvironment( 0f, 5f, 5f, 0f, 0f, 0f, 0f, 1f, 0f, this );

        Canvas3D canvas = Canvas3DFactory.createWindowed( 800, 600, "Robot room" );
        canvas.addWindowClosingListener( new WindowClosingRenderLoopEnder( this ) );
        env.addCanvas( canvas );

        InputSystem.getInstance().addInputListener(this);
        InputSystem.getInstance().registerNewKeyboardAndMouse( canvas.getPeer() );

        ResourceLocator resLoc = ResourceLocator.create( "test-resources/" );
        resLoc.createAndAddTSL( "textures" );

        Cylinder cyl = new Cylinder( 0.2f, 0.5f, 1f, true, 32, "robot2.jpg" );

        trans = new TransformGroup( new Transform3D(0f,0.5f,0f) );
        trans.addChild( cyl );

        TransformGroup tg = new TransformGroup();
        Room room = new Room();
        tg.addChild( room.getShape() );
        tg.addChild( trans );
        ObjectRotationInputHandler orih = new ObjectRotationInputHandler( tg );
        InputSystem.getInstance().addInputHandler( orih );

        BranchGroup bg = new BranchGroup();
        bg.addChild(tg);

        env.addPerspectiveBranch( bg );

    }

    public void onMouseButtonClicked(MouseButtonClickedEvent e, MouseButton button, int clickCount) {}
    public void onMouseButtonPressed(MouseButtonPressedEvent e, MouseButton button) {}
    public void onMouseButtonReleased(MouseButtonReleasedEvent e, MouseButton button) {}
    public void onMouseButtonStateChanged(MouseButtonEvent e, MouseButton button, boolean state) {}
    public void onMouseMoved(MouseMovedEvent e, int x, int y, int dx, int dy) {
        //if (e.getMouse().getButtonsState() != 0) {
        //}
    }
    public void onMouseWheelMoved(MouseWheelEvent e, int wheelDelta) {}

    public void onKeyReleased( KeyReleasedEvent e, Key key ) {
        switch (key.getKeyID()) {
        case LEFT:
            space.write("Turn",new Integer(-1),400,400f);
            break;
        case RIGHT:
            space.write("Turn",new Integer(1),400,400f);
            break;
        case UP:
            space.write("Forward",new Float(1f),400,400f);            
            break;
        case DOWN:
            space.write("Forward",new Float(-1f),400,400f);            
            break;
        case R:
            setPos(0,0);
            break;
        }
    }

    public static void main( String[] args ) throws Exception {
        Scene scene = new Scene();
        new Controller(scene);
        scene.begin();
    }

}
