package awa.imu;

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.GraphicsConfiguration;
import java.io.FileNotFoundException;
import javax.swing.JPanel;

import javax.media.j3d.*;
import javax.vecmath.*;

import com.sun.j3d.loaders.IncorrectFormatException;
import com.sun.j3d.loaders.ParsingErrorException;
import com.sun.j3d.loaders.Scene;
import com.sun.j3d.loaders.objectfile.ObjectFile;
import com.sun.j3d.utils.behaviors.mouse.MouseRotate;
import com.sun.j3d.utils.behaviors.mouse.MouseTranslate;
import com.sun.j3d.utils.behaviors.mouse.MouseZoom;
import com.sun.j3d.utils.geometry.Cylinder;
import com.sun.j3d.utils.universe.SimpleUniverse;

public class Main3dScreen extends JPanel
{
    
    private static final boolean spin = false;
    private static final boolean noTriangulate = false;
    private static final boolean noStripify = false;
    private static final double creaseAngle = 60.0;
    
    private SimpleUniverse u;
    private Canvas3D canvas3D;
    private TransformGroup sceneTrans;
    private BranchGroup rawDispGroup;
    private BranchGroup referenceGroup;
    private TransformGroup accXRot;
    private TransformGroup accYRot;
    private TransformGroup accXTrans;
    private TransformGroup accYTrans;
    private TransformGroup accZTrans;
    private RotationInterpolator airplaneRotator;
    private Transform3D view;
    private TransformGroup objTrans;
    
    private GyroRateIndicator gyroRoll;
    private GyroRateIndicator gyroPitch;
    private GyroRateIndicator gyroYaw;
    
    private IMUViewer parent;
    
    private String filename = "p51_mustang.obj";
    //private String filename = "galleon.obj";
    //private String filename = "beethoven.obj";
    
    public Main3dScreen(IMUViewer viewer)
    {
        parent = viewer;
        
        setLayout(new BorderLayout());
        
        createCanvas3D();
        add("Center", canvas3D);
    }
    
    public BranchGroup createSceneGraph()
    {
        // Create the root of the branch graph
        BranchGroup objRoot = new BranchGroup();
        //BranchGroup objRoot = createAxes();
        
        // Create a Transformgroup to scale all objects so they
        // appear in the scene.
        TransformGroup objScale = new TransformGroup();
        Transform3D t3d = new Transform3D();
        t3d.setScale(0.7);
        objScale.setTransform(t3d);
        objRoot.addChild(objScale);
        
        // Create the transform group node and initialize it to the
        // identity. Enable the TRANSFORM_WRITE capability so that
        // our behavior code can modify it at runtime. Add it to the
        // root of the subgraph.
        objTrans = new TransformGroup();
        objTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        objTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        objTrans.setCapability(Group.ALLOW_CHILDREN_EXTEND);
        objTrans.setCapability(Group.ALLOW_CHILDREN_READ);
        objTrans.setCapability(Group.ALLOW_CHILDREN_WRITE);
        objScale.addChild(objTrans);
        
        // Ground and Axes reference
        referenceGroup = new BranchGroup();
        referenceGroup.setCapability(BranchGroup.ALLOW_DETACH);
        referenceGroup.addChild(createAxes());
               
        // the Ground
        Ground ground = new Ground();
        referenceGroup.addChild(ground);
        objTrans.addChild(referenceGroup);
        
        // set the primary view perspective
        view = new Transform3D();
        resetView();
        
        // the airplane
        sceneTrans = new TransformGroup();
        sceneTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        sceneTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        objTrans.addChild(sceneTrans);
        
        // show raw measurements
        rawDispGroup = new BranchGroup();
        rawDispGroup.setCapability(BranchGroup.ALLOW_DETACH);
        //objTrans.addChild(accTrans);
        
        accXRot = new TransformGroup();
        accXRot.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        Transform3D rotx = new Transform3D();
        rotx.rotX(Math.PI/2);
        accXRot.setTransform(rotx);
        rawDispGroup.addChild(accXRot);
        
        accXTrans = new TransformGroup();
        accXTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        accXRot.addChild(accXTrans);

        accYRot = new TransformGroup();
        accYRot.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        Transform3D rotz = new Transform3D();
        rotz.rotZ(Math.PI/2);
        accYRot.setTransform(rotz);
        rawDispGroup.addChild(accYRot);
        
        accYTrans = new TransformGroup();
        accYTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        accYRot.addChild(accYTrans);

        accZTrans = new TransformGroup();
        accZTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        rawDispGroup.addChild(accZTrans);
        Appearance appX = new Appearance();
        //appX.setColoringAttributes(new ColoringAttributes(0.1f, 0.8f, 0.1f, ColoringAttributes.SHADE_GOURAUD));
        appX.setMaterial(new Material(new Color3f(0.8f, 0.2f, 0.2f), new Color3f(0.8f, 0.2f, 0.2f), new Color3f(0.5f, 0.5f, 0.5f), new Color3f(0.0f, 0.0f, 0.0f), 72));
        appX.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.BLENDED, 0.2f));
        Cylinder cylX = new Cylinder(0.15f, 1.5f, appX);
        accXTrans.addChild(cylX);
        Appearance appY = new Appearance();
        //appY.setColoringAttributes(new ColoringAttributes(0.1f, 0.1f, 0.8f, ColoringAttributes.SHADE_GOURAUD));
        appY.setMaterial(new Material(new Color3f(0.2f, 0.8f, 0.2f), new Color3f(0.2f, 0.8f, 0.2f), new Color3f(0.5f, 0.5f, 0.5f), new Color3f(0.0f, 0.0f, 0.0f), 72));
        appY.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.BLENDED, 0.2f));
        Cylinder cylY = new Cylinder(0.15f, 1.5f, appY);
        accYTrans.addChild(cylY);
        Appearance appZ = new Appearance();
        //appZ.setColoringAttributes(new ColoringAttributes(0.8f, 0.1f, 0.1f, ColoringAttributes.SHADE_GOURAUD));
        appZ.setMaterial(new Material(new Color3f(0.2f, 0.2f, 0.8f), new Color3f(0.2f, 0.2f, 0.8f), new Color3f(0.5f, 0.5f, 0.5f), new Color3f(0.0f, 0.0f, 0.0f), 72));
        appZ.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.BLENDED, 0.2f));
        Cylinder cylZ = new Cylinder(0.15f, 1.5f, appZ);
        accZTrans.addChild(cylZ);
        
        Color3f red = new Color3f(0.8f, 0.2f, 0.2f);
        gyroYaw = new GyroRateIndicator(red);
        TransformGroup rollRot = new TransformGroup();
        rollRot.addChild(gyroYaw);
        rollRot.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        Transform3D rotxy = new Transform3D();
        rotxy.rotY(Math.PI/2);
        rotxy.mul(rotx);
        rollRot.setTransform(rotxy);
        rawDispGroup.addChild(rollRot);
        
        Color3f green = new Color3f(0.2f, 0.8f, 0.2f);
        gyroPitch = new GyroRateIndicator(green);
        TransformGroup pitchRot = new TransformGroup();
        pitchRot.addChild(gyroPitch);
        pitchRot.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        Transform3D roty = new Transform3D();
        roty.rotY(Math.PI/2);
        pitchRot.setTransform(roty);
        rawDispGroup.addChild(pitchRot);
        
        Color3f blue = new Color3f(0.2f, 0.2f, 0.8f);
        gyroRoll = new GyroRateIndicator(blue);
        rawDispGroup.addChild(gyroRoll);

        // load the airplane object
        int flags = ObjectFile.RESIZE;
        if (!noTriangulate)
            flags |= ObjectFile.TRIANGULATE;
        if (!noStripify)
            flags |= ObjectFile.STRIPIFY;
        ObjectFile f = new ObjectFile(flags, (float) (creaseAngle * Math.PI / 180.0));
        Scene s = null;
        try {
            s = f.load(filename);
        } catch (FileNotFoundException e) {
            System.err.println(e);
            System.exit(1);
        } catch (ParsingErrorException e) {
            System.err.println(e);
            System.exit(1);
        } catch (IncorrectFormatException e) {
            System.err.println(e);
            System.exit(1);
        }
        
        sceneTrans.addChild(s.getSceneGroup());
        
        BoundingSphere bounds = new BoundingSphere(new Point3d(0.0, 0.0, 0.0), 100.0);
        
        // rotate the airplane only
        Transform3D yAxis1 = new Transform3D();
        //yAxis1.rotZ(Math.toRadians(90));  // makes it rotate around z-axis
        Alpha rotationAlpha1 = new Alpha(-1, Alpha.INCREASING_ENABLE, 0, 0, 4000, 0, 0, 0, 0, 0);
        
        airplaneRotator = new RotationInterpolator(rotationAlpha1, sceneTrans, yAxis1, 0.0f, (float) Math.PI * 2.0f);
        airplaneRotator.setSchedulingBounds(bounds);
        sceneTrans.addChild(airplaneRotator);
        
        if (spin) {
            /*
            Transform3D yAxis = new Transform3D();
            Alpha rotationAlpha = new Alpha(-1, Alpha.INCREASING_ENABLE, 0, 0, 4000, 0, 0, 0, 0, 0);
            
            RotationInterpolator rotator = new RotationInterpolator(rotationAlpha, objTrans, yAxis, 0.0f, (float) Math.PI * 2.0f);
            rotator.setSchedulingBounds(bounds);
            objTrans.addChild(rotator);
            */
        } else {
            // Create the rotate behavior node
            MouseRotate behavior = new MouseRotate();
            behavior.setTransformGroup(objTrans);
            objTrans.addChild(behavior);
            behavior.setSchedulingBounds(bounds);
            
            // Create the zoom behavior node
            MouseZoom behavior2 = new MouseZoom();
            behavior2.setTransformGroup(objTrans);
            objTrans.addChild(behavior2);
            behavior2.setSchedulingBounds(bounds);
            
            // Create the translate behavior node
            MouseTranslate behavior3 = new MouseTranslate();
            behavior3.setTransformGroup(objTrans);
            objTrans.addChild(behavior3);
            behavior3.setSchedulingBounds(bounds);
        }
        
        // Set up the background
        Color3f bgColor = new Color3f(0.05f, 0.05f, 0.5f);
        Background bgNode = new Background(bgColor);
        bgNode.setApplicationBounds(bounds);
        objRoot.addChild(bgNode);
        
        // Set up the ambient light
        Color3f ambientColor = new Color3f(0.1f, 0.1f, 0.1f);
        AmbientLight ambientLightNode = new AmbientLight(ambientColor);
        ambientLightNode.setInfluencingBounds(bounds);
        objRoot.addChild(ambientLightNode);
        
        // Set up the directional lights
        Color3f light1Color = new Color3f(1.0f, 0.8f, 0.2f);
        Vector3f light1Direction = new Vector3f(4.0f, -7.0f, -12.0f);
        Color3f light2Color = new Color3f(0.8f, 0.4f, 0.8f);
        Vector3f light2Direction = new Vector3f(-6.0f, -2.0f, -1.0f);
        
        DirectionalLight light1 = new DirectionalLight(light1Color, light1Direction);
        light1.setInfluencingBounds(bounds);
        objRoot.addChild(light1);
        
        DirectionalLight light2 = new DirectionalLight(light2Color, light2Direction);
        light2.setInfluencingBounds(bounds);
        objRoot.addChild(light2);
        
        return objRoot;
    }
    
    private BranchGroup createAxes()
    {
        BranchGroup axisBG = new BranchGroup();
        
        /*
         Cylinder zaxis = new Cylinder(0.01f, 3.0f);
         axisBG.addChild(zaxis);
         Cone zcone = new Cone(0.01f, 0.1f);
         axisBG.addChild(zcone); */
        
        // create line for X axis
        LineArray axisXLines = new LineArray(2, GeometryArray.COORDINATES  );
        axisBG.addChild(new Shape3D(axisXLines));
        
        axisXLines.setCoordinate(0, new Point3f(-1.5f, 0.0f, 0.0f));
        axisXLines.setCoordinate(1, new Point3f( 1.5f, 0.0f, 0.0f));
        
        final Color3f red   = new Color3f(Color.red);
        final Color3f green = new Color3f(0.0f, 1.5f, 0.0f);
        final Color3f blue  = new Color3f(0.0f, 0.0f, 1.5f);
        
        // create line for Y axis
        LineArray axisYLines = new LineArray(2, 
                GeometryArray.COORDINATES | GeometryArray.COLOR_3 );
        axisBG.addChild(new Shape3D(axisYLines));
        
        axisYLines.setCoordinate(0, new Point3f( 0.0f,-1.5f, 0.0f));
        axisYLines.setCoordinate(1, new Point3f( 0.0f, 1.5f, 0.0f));
        
        axisYLines.setColor(0, green);
        axisYLines.setColor(1, blue);
        
        // create line for Z axis
        Point3f z1 = new Point3f( 0.0f, 0.0f,-1.5f);
        Point3f z2 = new Point3f( 0.0f, 0.0f, 1.5f);
        
        /*
        LineArray axisZLines = new LineArray(10, 
                LineArray.COORDINATES  | LineArray.COLOR_3
        );
        axisBG.addChild(new Shape3D(axisZLines));
        
        axisZLines.setCoordinate(0, z1);
        axisZLines.setCoordinate(1, z2);
        axisZLines.setCoordinate(2, z2);
        axisZLines.setCoordinate(3, new Point3f( 0.1f, 0.1f, 1.4f));
        axisZLines.setCoordinate(4, z2);
        axisZLines.setCoordinate(5, new Point3f(-0.1f, 0.1f, 1.4f));
        axisZLines.setCoordinate(6, z2);
        axisZLines.setCoordinate(7, new Point3f( 0.1f,-0.1f, 1.4f));
        axisZLines.setCoordinate(8, z2);
        axisZLines.setCoordinate(9, new Point3f(-0.1f,-0.1f, 1.4f));
        
        Color3f colors[] = new Color3f[10];        //array of colors
        
        colors[0] = new Color3f(0.0f, 1.0f, 1.0f); //set the first color
        for(int v = 1; v < 10; v++){               //set the remaining colors
            colors[v] = red;
        }*/
        
        LineArray axisZLines = new LineArray(2, 
                GeometryArray.COORDINATES  | GeometryArray.COLOR_3
        );
        axisBG.addChild(new Shape3D(axisZLines));
        
        axisZLines.setCoordinate(0, z1);
        axisZLines.setCoordinate(1, z2);
        axisZLines.setColor(0, red);           // add colors to geometry
        axisZLines.setColor(1, red); 
        
        // Create a Text2D leaf node, add it to the scene graph.
        //Text2D text2D = new Text2D("2D text is a textured polygon",
        //        new Color3f(0.9f, 1.0f, 1.0f), "Helvetica", 18, Font.ITALIC);
        //axisBG.addChild(text2D);
        
        return axisBG;
    }
    
    
    // Create the Canvas3D (both on-screen and off-screen)
    private void createCanvas3D()
    {
        // Create Canvas3D
        GraphicsConfiguration config = SimpleUniverse.getPreferredConfiguration();
        
        canvas3D = new Canvas3D(config);
        //canvas3D.setSize(this.getSize());
        
        // Create a simple scene and attach it to the virtual universe
        BranchGroup scene = createSceneGraph();
        u = new SimpleUniverse(canvas3D);
        // This will move the ViewPlatform back a bit so the
        // objects in the scene can be viewed.
        u.getViewingPlatform().setNominalViewingTransform();
        u.addBranchGraph(scene);
    }
    
    public void setDemo(boolean spin)
    {
        //System.out.println("setDemo(" + spin + ")");
        airplaneRotator.setEnable(spin);
    }
    
    public void setShowRaw(boolean show)
    {
        if (show) {
            objTrans.addChild(rawDispGroup);
            objTrans.removeChild(referenceGroup);
            setRotation(0.0, 0.0, 0.0);
        } else {
            objTrans.removeChild(rawDispGroup);
            if (objTrans.indexOfChild(referenceGroup) < 0) objTrans.addChild(referenceGroup);
        }
    }
    
    /**
     * simple attempt to set attitude from accelerator values
     * @param x
     * @param y
     * @param z
     */
    public void setXYZ(double ax, double ay, double az)
    {
        double gAbs = Math.sqrt(ax*ax + ay*ay + az*az);
        
        // prevent division by zero
        if (gAbs == 0.0) gAbs = 1.0;
        
        double phi = Math.atan2(ay, az);    // bank angle
        double theta = Math.asin(ax / gAbs);
                
        setRotation(phi, theta, 0.0);
    }
    
    public void showRaw(double ax, double ay, double az, double roll, double pitch, double yaw)
    {
        Transform3D scaleX = new Transform3D();
        scaleX.setScale(new Vector3d(1.0, -ax, 1.0));
        scaleX.setTranslation(new Vector3d(0, -ax/1.4, 0));
        accXTrans.setTransform(scaleX);
        Transform3D scaleY = new Transform3D();
        scaleY.setScale(new Vector3d(1.0, ay, 1.0));
        scaleY.setTranslation(new Vector3d(0, ay/1.4, 0));
        accYTrans.setTransform(scaleY);
        Transform3D scaleZ = new Transform3D();
        scaleZ.setScale(new Vector3d(1.0, az, 1.0));
        scaleZ.setTranslation(new Vector3d(0, az/1.4, 0));
        accZTrans.setTransform(scaleZ);
        
        gyroRoll.showRate(roll);
        gyroPitch.showRate(pitch);
        gyroYaw.showRate(yaw);
    }
    
    public void addRotation(double pitch, double roll, double yaw) 
    {
        Transform3D curr = new Transform3D();
        sceneTrans.getTransform(curr);
        Transform3D rot = new Transform3D();
        rot.rotZ(roll);
        curr.mul(rot);
        rot = new Transform3D();
        rot.rotX(pitch);
        curr.mul(rot);
        rot = new Transform3D();
        rot.rotY(yaw);
        curr.mul(rot);
        sceneTrans.setTransform(curr);

    }
    
    /**
     * rotates the object in the NED-Frame with given Euler angles
     */
    public void setRotation(double phi, double theta, double psi) 
    {
        parent.displayRotation(phi, theta, psi);
        
        // initial rotate the plane so that the nose points north
        Transform3D preset = new Transform3D();
        preset.rotY(Math.PI/2.0);
        
        Transform3D rot = new Transform3D();
        rot.rotY(-psi);
        preset.mul(rot);
        rot = new Transform3D();
        rot.rotZ(theta);
        preset.mul(rot);
        rot = new Transform3D();
        rot.rotX(phi);
        preset.mul(rot);
        sceneTrans.setTransform(preset);
    }
    
    public void resetView()
    {
        Point3d eye = new Point3d(0.2,0.5,1.0);
        Point3d center = new Point3d(0,0,0);
        Vector3d up = new Vector3d(-0.4,0,-2);
        view.lookAt(eye, center, up);
        objTrans.setTransform(view);

    }

}