package visualization;

import com.jme3.app.SimpleApplication;
import com.jme3.light.DirectionalLight;
import com.jme3.light.SpotLight;
import com.jme3.math.ColorRGBA;
import com.jme3.math.FastMath;
import com.jme3.math.Matrix3f;
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.scene.shape.Box;
import com.jme3.ui.Picture;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
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.3f;
    private static final float DETECTION = 4.f;
    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 float[] last_rotation;
    private int state = 0;
    private boolean first = true;
    private Spatial model_object;
    private boolean busy_action = false;
    private Action action;
    boolean wireframe = false;
    
    private static final int X = 0;
    private static final int Y = 1;
    private static final int Z = 2;
    
    private boolean reverse = false;
    private int iter_last_action = 0;
    private int iter_dec = 0;
    private int time_between_actions = 0;
    private int wait_iter = 0;
    private boolean stop = false;
    private boolean between = false;
    private ColorRGBA background;
    private String model;
    private boolean properties = false;
    private Server server;
    private Picture pic;
    
    private Vector3f initial_scale = null;
    boolean decelerate = false;
    File f;
    BufferedWriter bw;
    
    public GUI(Server s) {
        super();
        server = s;
    }
    
    @Override
    public void destroy() {
        try {
            bw.close();
        } catch (IOException ex) {
            Logger.getLogger(GUI.class.getName()).log(Level.SEVERE, null, ex);
        }
        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);
            }
        }
        
        f = new File("out.txt");
        FileWriter fw;
        try {
            fw = new FileWriter(f);
            bw = new BufferedWriter(fw);
        } catch (IOException ex) {
            Logger.getLogger(GUI.class.getName()).log(Level.SEVERE, null, ex);
        }
        
        
        //init vars
        
        acc_angles = new float[3];
        acc_angles_old = new float[3];
        complementary = new float[3];
        complementary_old = new float[3];
        linear_acc = new float[3];
        gravity = new float[3];
        gravity_old = new float[3];
        last_rotation = new float[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(0, 0, -1).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;
    }
    
    private void write(Vector3f tmp){
        try {
            bw.write(tmp.x + "," + tmp.y +"," + tmp.z +"\n");
        } catch (IOException ex) {
            Logger.getLogger(GUI.class.getName()).log(Level.SEVERE, null, ex);
        }
    }
    
    /* 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 = acc;
                        complementary_old[0] = complementary_old[1] = complementary_old[2] = 0;
                        complementary[0] = complementary[1] = complementary[2] = 0;
                        System.out.println("Complementary: " + complementary[0] +", "+ complementary[1] +", "+ complementary[2]);
                        acc_angles_old[0] = acc_angles_old[1] = acc_angles_old[2] = 0;
                        first = false;
                        between = true;
                        time_between_actions = 20;
                    } 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[0] = BETA * gravity_old[0] + (1 - BETA) * acc[0];
                        gravity[1] = BETA * gravity_old[1] + (1 - BETA) * acc[1];
                        gravity[2] = BETA * gravity_old[2] + (1 - BETA) * acc[2];
                        
                        
                        linear_acc[0] = acc[0] - gravity[0];
                        linear_acc[1] = acc[1] - gravity[1];
                        linear_acc[2] = acc[2] - gravity[2];
                        
                        //get acc angles
                        float a = FastMath.sqrt(FastMath.sqr(acc[0]) + FastMath.sqr(acc[1]) + FastMath.sqr(acc[2]));
                        acc_angles[0] = FastMath.acos(acc[0] / a);
                        acc_angles[1] = FastMath.acos(acc[1] / a);
                        acc_angles[2] = FastMath.acos(acc[2] / a);
                        
                        //apply complementary filter
                        complementary[0] = (1 - ALPHA) * (complementary_old[0] + gyro[0] * TIME_CONSTANT) + ALPHA * acc_angles[0];
                        complementary[1] = (1 - ALPHA) * (complementary_old[1] + gyro[1] * TIME_CONSTANT) + ALPHA * acc_angles[1];
                        complementary[2] = (1 - ALPHA) * (complementary_old[2] + gyro[2] * TIME_CONSTANT) + ALPHA * acc_angles[2];
                        
                        //copy new values for next iter.
                        acc_angles_old = acc_angles;
                        gravity_old = gravity;
                        complementary_old = complementary;
                        
                        
                        Vector3f ori = new Vector3f(complementary[0], complementary[1], complementary[2]);
                        
                        
                        float roll = 0, yaw = 0, pitch = 0;
                        
                        
                        pitch = FastMath.atan (gravity[0] / FastMath.sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
                        roll = FastMath.atan (gravity[1] / FastMath.sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
                        
                        
                        Quaternion q = new Quaternion();
                        q = q.fromAngles(roll, yaw, pitch);
                        
                        model_object.setLocalRotation(q);
                        
                       
                       
                        
                    }
                    
                    
                    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 state){
        
        if(state == 1){//acc
            return rad;
        } else if(state == -1){ //dec
            return rad/4;
        } else{ //cte
            return rad/2;
        }
        /*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 state){
        if(state == 1){//acc
            return units;
        } else if(state == -1){ //dec
            return units/4;
        } else{ //cte
            return units/2;
        }
        /*
        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;
        int sign;
        public Action(int type, float value, String axis){
            this.type = type;
            this.value = value;
            this.axis = axis;
            this.sign = (int) (value/FastMath.abs(value));
        }
    }
    
    

}
