package visualization;

import com.jme3.app.SimpleApplication;
import com.jme3.light.DirectionalLight;
import com.jme3.math.ColorRGBA;
import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import com.jme3.scene.Geometry;
import com.jme3.scene.SceneGraphVisitor;
import com.jme3.scene.Spatial;
import com.jme3.ui.Picture;
import java.text.ParseException;
import java.util.concurrent.Callable;
import java.util.logging.Level;
import java.util.logging.Logger;
import server.Server;
import utils.Buffer;
import utils.Data;

public class GUI extends SimpleApplication {
    
    protected static Geometry player;
    protected boolean new_pos = false;
    private Buffer buffer = new Buffer(500000);
    public static final float TIME_CONSTANT = 0.02f;
    public static final float GRAVITY = 9.80665f;
    private static final float ALPHA = 0.95f;
    private static final float BETA = 0.90f;
    private static final int STATE_mX = -1;
    private static final int STATE_pX = 1;
    private static final int STATE_mY = -2;
    private static final int STATE_pY = 2;
    private static final int STATE_mZ = -3;
    private static final int STATE_pZ = 3;
    private static final float THRESHOLD = 0.1f;
    private static final float DETECTION = 3;
    private static final int TOTAL_FRAMES = 40;
    private float[][] acc_angles;
    private float[][] acc_angles_old;
    private float[][] complementary;
    private float[][] complementary_old;
    private float[][] linear_acc;
    private float[][] gravity;
    private float[][] gravity_old;
    private int[] state = new int[2];
    private boolean first = true;
    private Spatial model_object;
    private boolean busy_action = false;
    private Action action;
    boolean wireframe = false;
    
    private static final int ROT = 0;
    private static final int PAN = 1;
    private static final int ZOOM = 2;
    
    private boolean reverse = false;
    private int iter_last_action = 0;
    private ColorRGBA background;
    private String model;
    private boolean properties = false;
    private Server server;
    private Picture pic;
    
    private Vector3f initial_scale = null;
    
    public GUI(Server s) {
        super();
        server = s;
    }
    
    @Override
    public void destroy() {
        server.stopReading();
        super.destroy();
    }
    
    @Override
    public void simpleInitApp() {
        setDisplayStatView(false);
        Logger.getLogger("com.jme3").setLevel(Level.SEVERE);
        
        
        JFrameProperties jf = new JFrameProperties(this);
        jf.setVisible(true);
        while (!properties) {
            try {
                Thread.sleep(100);
            } catch (InterruptedException ex) {
                Logger.getLogger(GUI.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
        
        
        //init vars
        
        acc_angles = new float[2][3];
        acc_angles_old = new float[2][3];
        complementary = new float[2][3];
        complementary_old = new float[2][3];
        linear_acc = new float[2][3];
        gravity = new float[2][3];
        gravity_old = new float[2][3];
        
        System.out.println(background.toString());
        viewPort.setBackgroundColor(background);
        
        if (!model.equals("")) {
            int i = model.indexOf("Models/");
            model = model.substring(i);
            model_object = assetManager.loadModel(model);
        } else {
            model_object = assetManager.loadModel("Models/Teapot/Teapot.obj");
        }
        
        model_object.setLocalTranslation(0, -100, -200);
        
        initial_scale = model_object.getLocalScale();
        rootNode.attachChild(model_object);
        
        flyCam.setEnabled(false);
        
        DirectionalLight sun = new DirectionalLight();
        sun.setDirection(new Vector3f(1, 0, -2).normalizeLocal());
        sun.setColor(ColorRGBA.White.mult(1.2f));
        
        rootNode.addLight(sun);
        
		sun = new DirectionalLight();
        sun.setDirection(new Vector3f(0, 1, 0).normalizeLocal());
        sun.setColor(ColorRGBA.White.mult(0.5f));

        rootNode.addLight(sun);

        sun = new DirectionalLight();
        sun.setDirection(new Vector3f(0, -1, 0).normalizeLocal());
        sun.setColor(ColorRGBA.White.mult(0.5f));

        rootNode.addLight(sun);


        pic = new Picture("HUD Picture");
        
        cam.setLocation(cam.getLocation().add(0, -50, 0));
        
        
    }
    
    
    
    private void toggleWireframe(){
        wireframe = !wireframe; // toggle boolean
        rootNode.depthFirstTraversal(new SceneGraphVisitor() {
            public void visit(Spatial spatial) {
                if (spatial instanceof Geometry) {
                    ((Geometry)spatial).getMaterial().getAdditionalRenderState().setWireframe(wireframe);
                }
            }
        });
    }
    public void setProperties(String color, String model) {
        if (color.equals("White")) {
            this.background = ColorRGBA.White;
        } else if (color.equals("Green")) {
            this.background = ColorRGBA.Green;
        } else if (color.equals("Gray")) {
            this.background = ColorRGBA.Gray;
        } else {
            this.background = ColorRGBA.Black;
        }
        this.model = model;
        properties = true;
    }
    
    /* This is the update loop */
    @Override
    public void simpleUpdate(float tpf) {
        
        
        if (new_pos) {
            
            
            
            this.enqueue(new Callable<Object>() {
                @SuppressWarnings("empty-statement")
                public Object call() throws Exception {
                    
                    Data d = (Data) buffer.get();
                    if (d == null) {
                        return null;
                    }
                    float[] acc = d.getAcc();
                    float[] gyro = d.getGyro();
                    float[] mag = d.getMag(); // will be never used
                    int dev = d.getDev();
                    
                    if (first) {
                        gravity_old[dev] = acc;
                        complementary_old[dev][0] = complementary_old[dev][1] = complementary_old[dev][2] = 0;
                        complementary[dev][0] = complementary[dev][1] = complementary[dev][2] = 0;
                        System.out.println("Complementary: " + complementary[dev][0] +", "+ complementary[dev][1] +", "+ complementary[dev][2]);
                        acc_angles_old[dev][0] = acc_angles_old[dev][1] = acc_angles_old[dev][2] = 0;
                        first = false;
                    } else {
                        //convert gyro to rads
                        gyro[0] = gyro[0] * FastMath.DEG_TO_RAD;
                        gyro[1] = gyro[1] * FastMath.DEG_TO_RAD;
                        gyro[2] = gyro[2] * FastMath.DEG_TO_RAD;
                        
                        //get gravity
                        gravity[dev][0] = BETA * gravity_old[dev][0] + (1 - BETA) * acc[0];
                        gravity[dev][1] = BETA * gravity_old[dev][1] + (1 - BETA) * acc[1];
                        gravity[dev][2] = BETA * gravity_old[dev][2] + (1 - BETA) * acc[2];
                        
                        
                        linear_acc[dev][0] = acc[0] - gravity[dev][0];
                        linear_acc[dev][1] = acc[1] - gravity[dev][1];
                        linear_acc[dev][2] = acc[2] - gravity[dev][2];
                        
                        //get acc angles
                        float a = FastMath.sqrt(FastMath.sqr(acc[0]) + FastMath.sqr(acc[1]) + FastMath.sqr(acc[2]));
                        acc_angles[dev][0] = FastMath.acos(acc[0] / a);
                        acc_angles[dev][1] = FastMath.acos(acc[1] / a);
                        acc_angles[dev][2] = FastMath.acos(acc[2] / a);
                        
                        //apply complementary filter
                        complementary[dev][0] = (1 - ALPHA) * (complementary_old[dev][0] + gyro[0] * TIME_CONSTANT) + ALPHA * acc_angles[dev][0];
                        complementary[dev][1] = (1 - ALPHA) * (complementary_old[dev][1] + gyro[1] * TIME_CONSTANT) + ALPHA * acc_angles[dev][1];
                        complementary[dev][2] = (1 - ALPHA) * (complementary_old[dev][2] + gyro[2] * TIME_CONSTANT) + ALPHA * acc_angles[dev][2];
                        
                        //copy new values for next iter.
                        acc_angles_old = acc_angles;
                        gravity_old = gravity;
                        complementary_old = complementary;
                        
                        
                        
                        Vector3f ori = new Vector3f(complementary[dev][0], complementary[dev][1], complementary[dev][2]);
                        
                        
                        if (FastMath.abs(ori.getX() - FastMath.PI) < THRESHOLD){
                            state[dev] = STATE_mX;
                        } else if(FastMath.abs(ori.getX()) < THRESHOLD){
                            state[dev] = STATE_pX;
                        } else if (FastMath.abs(ori.getY() - FastMath.PI) < THRESHOLD){
                            state[dev] = STATE_mY;
                        } else if(FastMath.abs(ori.getY()) < THRESHOLD){
                            state[dev] = STATE_pY;
                        } else if (FastMath.abs(ori.getZ() - FastMath.PI) < THRESHOLD){
                            state[dev] = STATE_mZ;
                        } else if(FastMath.abs(ori.getZ()) < THRESHOLD){
                            state[dev] = STATE_pZ;
                        }
                       
                        
                    }
                    if (!busy_action) {
                        if (state[0] == STATE_mY && state[1] == STATE_mY) { //ROT
                            displayImage("rotate");
                            
                            float x_acc = linear_acc[0][0];
                            float y_acc = linear_acc[1][1];
                            
                            
                            if (reverse) {
                                reverse = false;
                            } else {
                                if (FastMath.abs(x_acc) > DETECTION) {
                                    float sign = -1 * (x_acc / FastMath.abs(x_acc));
                                    action = new Action(ROT, (float)(sign * 0.15), "x");
                                    
                                    busy_action = true;
                                    iter_last_action = 0;
                                    
                                } else if (FastMath.abs(y_acc) > DETECTION) {
                                    float sign = -1 * (y_acc / FastMath.abs(y_acc));
                                    action = new Action(ROT, (float)(sign * 0.15), "y");
                                    busy_action = true;
                                    iter_last_action = 0;
                                }
                            }
                            
                            
                            
                        } else if (state[0] == STATE_mY && state[1] == STATE_mZ) { //zoom
                            displayImage("zoom");
                            
                            float l_acc = linear_acc[1][1];
                            
                            if (FastMath.abs(l_acc) > DETECTION) {
                                if (reverse) {
                                    reverse = false;
                                } else {
                                    float sign = -1 * (l_acc / FastMath.abs(l_acc));
                                    
                                    action = new Action(ZOOM, (float)(sign*4), "");
                                    reverse = true;
                                }
                                busy_action = true;
                                iter_last_action = 0;
                                
                            }
                            
                        } else if (state[1] == STATE_mY && state[0] == STATE_mZ) { //pan
                            displayImage("pan");
                            
                            float x_acc = linear_acc[0][0];
                            float y_acc = linear_acc[1][1];
                            
                            
                            if (reverse) {
                                reverse = false;
                            } else {
                                if (FastMath.abs(x_acc) > DETECTION) {
                                    float sign = -1 * (x_acc / FastMath.abs(x_acc));
                                    
                                    action = new Action(PAN, (float)(sign * 10), "x");
                                    busy_action = true;
                                    iter_last_action = 0;
                                    
                                } else if (FastMath.abs(y_acc) > DETECTION) {
                                    float sign = -1 * (y_acc / FastMath.abs(y_acc));
                                    
                                    action = new Action(PAN, (float)(sign * 10), "y");
                                    busy_action = true;
                                    iter_last_action = 0;
                                }
                            }
                            
                        } else if (state[0] == STATE_mZ && state[1] == STATE_mZ) { //center
                            displayImage("reset");
                            model_object.setLocalRotation(Quaternion.IDENTITY);
                            model_object.center();
                            model_object.setLocalScale(initial_scale);
                            model_object.setLocalTranslation(0, -100, -200);
                        } else if((state[0] == STATE_mY && state[1] == STATE_pZ)){
                            displayImage("wire");
                            if(FastMath.abs(linear_acc[0][0]) > DETECTION){
                                toggleWireframe();
                                busy_action = true;
                                action = null;
                                iter_last_action = 0;
                            }
                            
                        } else {
                            displayImage(null);
                        }
                    } else {
                        iter_last_action++;
                        if(action != null){
                            switch(action.type){
                                case ROT:
                                    if(action.axis.equals("x")){
                                        model_object.rotate(0.f, calcRotation(action.value, TOTAL_FRAMES, iter_last_action), 0.f);
                                    }else{
                                        model_object.rotate(calcRotation(action.value, TOTAL_FRAMES, iter_last_action), 0.f, 0.f);
                                    }
                                    break;
                                case PAN:
                                    if(action.axis.equals("x")){
                                        model_object.move(calcDesp(action.value, TOTAL_FRAMES, iter_last_action), 0.f, 0.f);
                                    }else{
                                        model_object.move(0.f, calcDesp(action.value, TOTAL_FRAMES, iter_last_action), 0.f);
                                    }
                                    break;
                                case ZOOM:
                                    Vector3f scale = model_object.getLocalScale();
                                    float val = calcDesp(action.value, TOTAL_FRAMES, iter_last_action);
                                    scale = scale.add(val * 0.2f, val * 0.2f, val * 0.2f);
                                    model_object.setLocalScale(scale);
                                    
                                    break;
                                default:
                                    busy_action = false;
                                    break;
                            }
                        }
                        if (iter_last_action > TOTAL_FRAMES) {
                            busy_action = false;
                            action = null;
                        }
                    }
                    
                    
                    return null;
                }
            });
            
            
        }
        
        
    }
    /*
     * V1 = 2*V0
     *
     * t/8 --- t*3/4 --- t/8
     *  v0 --- v1  --- v0
     *
     * v0 * t/4  + v1*3/4t = rot
     * v0*7/4 * t = rot
     * v0 = rot*4 / 7*t
     *
     */
    private float calcRotation(float rad, int total_frames, int current_frame){
        float v_0 = (rad * 4) / (7*total_frames);
        float v_1 = v_0 * 2;
        
        if( current_frame < total_frames/8 || current_frame > total_frames*7/8){
            return v_0;
        }
        return v_1;
    }
    
    /*
     * V1 = 2*V0
     *
     * t/8 --- t*3/4 --- t/8
     *  v0 --- v1  --- v0
     *
     * v0 * t/4  + v1*3/4t = rot
     * v0*7/4 * t = rot
     * v0 = rot*4 / 7*t
     *
     */
    private float calcDesp(float units, int total_frames, int current_frame){
        float v_0 = (units * 4) / (7*total_frames);
        float v_1 = v_0 * 2;
        
        if( current_frame < total_frames/8 || current_frame > total_frames*7/8){
            return v_0;
        }
        return v_1;
    }
    
    /*
     * V1 = 2*V0
     *
     * t/8 --- t*3/4 --- t/8
     *  v0 --- v1  --- v0
     *
     * v0 * t/4  + v1*3/4t = rot
     * v0*7/4 * t = rot
     * v0 = rot*4 / 7*t
     *
     */
    private float calcZoom(float units, int total_frames, int current_frame){
        float v_0 = (units * 4) / (7*total_frames);
        float v_1 = v_0 * 2;
        
        if( current_frame < total_frames/8 || current_frame > total_frames*7/8){
            return v_0;
        }
        return v_1;
    }
    
    private void displayImage(String state) {
        if (state == null) {
            guiNode.detachChild(pic);
        } else {
            String icon = "Icons/" + state + ".png";
            guiNode.detachChild(pic);
            pic.setImage(assetManager, icon, true);
            pic.setHeight(30);
            pic.setWidth(30);
            pic.setPosition(40, 40);
            guiNode.attachChild(pic);
        }
    }
    
    /**
     * Adds a new acceleration value
     *
     * @param int x
     * @param int y
     * @param int z
     */
    public void addValues(float[] acc, float[] gyro, float[] mag, String time, int dev) throws InterruptedException {
        try {
            Data d = new Data(acc, gyro, mag, time, dev);
            
            buffer.put(d);
            
            new_pos = true;
            
        } catch (ParseException ex) {
            Logger.getLogger(GUI.class.getName()).log(Level.SEVERE, null, ex);
        }
    }
    
    class Action{
        int type;
        float value;
        String axis;
        public Action(int type, float value, String axis){
            this.type = type;
            this.value = value;
            this.axis = axis;
        }
    }
    
    

}
