/*

  Copyright (C) 2017 Elphel Inc.

  License: GPLv3

  https://www.elphel.com

*/
/**
 * @file -
 * @brief -
 *
 * @copyright Copyright (C) 2017 Elphel Inc.
 * @author Oleg Dzhimiev <oleg@elphel.com>
 *
 * @licstart  The following is the entire license notice for the
 * JavaScript code in this page.
 *
 *   The JavaScript code in this page is free software: you can
 *   redistribute it and/or modify it under the terms of the GNU
 *   General Public License (GNU GPL) as published by the Free Software
 *   Foundation, either version 3 of the License, or (at your option)
 *   any later version.  The code is distributed WITHOUT ANY WARRANTY;
 *   without even the implied warranty of MERCHANTABILITY or FITNESS
 *   FOR A PARTICULAR PURPOSE.  See the GNU GPL for more details.
 *
 *   As additional permission under GNU GPL version 3 section 7, you
 *   may distribute non-source (e.g., minimized or compacted) forms of
 *   that code without the copy of the GNU GPL normally required by
 *   section 4, provided you include this license notice and a URL
 *   through which recipients can access the Corresponding Source.
 *
 *  @licend  The above is the entire license notice
 *  for the JavaScript code in this page.
 */

function x3dom_getViewTranslation(elem){

    var m = elem.runtime.viewMatrix().inverse();
    var tr = m.e3();

    return tr;

}

/**
 * get position and orientation in the 3D scene defined by mouse's canvas x,y
 */
function x3dom_getXYPosOr(cnvx,cnvy,round){

    var elem = Scene.element;

    var x,y,z;
    var az,el,sk;
    var id;

    var dist_xyz = 1000;
    var dist_xz = 1000;

    var shootRay = elem.runtime.shootRay(cnvx,cnvy);

    var valid_distance = true;

    if (shootRay.pickPosition != null){

        var index = Scene.highlighted_marker_index;

        if (index==null){
            if ((Scene.draggedTransformNode!=undefined)&&(Scene.draggedTransformNode!=null)){
                var sphere = Scene.draggedTransformNode.parent().parent();
                index = parseInt(sphere.attr("id").substr(7));
            }
        }

        if ((index==null)||(Data.markers[index]==undefined)){

            x = shootRay.pickPosition.x;
            y = shootRay.pickPosition.y;
            z = shootRay.pickPosition.z;

            dist_xz  = Math.sqrt(x*x+z*z);

        }else{

            x = Data.markers[index].x;
            y = Data.markers[index].y;
            z = Data.markers[index].z;

            dist_xz  = Data.markers[index].d_x3d;
            if (isNaN(dist_xz)){
                dist_xz  = Math.sqrt(x*x+z*z);
            }
        }

        dist_xyz = Math.sqrt(y*y+dist_xz*dist_xz);

        id = $(shootRay.pickObject).attr("id");

    }else{

        var viewingRay = elem.runtime.getViewingRay(cnvx,cnvy);

        x = viewingRay.dir.x;
        y = viewingRay.dir.y;
        z = viewingRay.dir.z;

        dist_xz = null;
        dist_xyz = null;

        valid_distance = false;
    }

    var R0 = Data.camera.Matrices.R0;
    var p_w = new x3dom.fields.SFVec3f(x,y,z);
    var p_rw = R0.multMatrixVec(p_w);

    if (valid_distance){
        dist_xz = Math.sqrt(p_rw.x*p_rw.x+p_rw.z*p_rw.z);
        dist_xyz = Math.sqrt(p_rw.y*p_rw.y+dist_xz*dist_xz);
    }else{
        dist_xz = null;
        dist_xyz = null;
    }

    az = Math.atan2(p_rw.x,-p_rw.z)*180/Math.PI;
    az = (az+360)%360;
    el = Math.atan2(p_rw.y,Math.sqrt(p_rw.x*p_rw.x+p_rw.z*p_rw.z))*180/Math.PI;
    sk = 0;

    var result = {
        x: !round? x : x.toFixed(2),
        y: !round? y : y.toFixed(2),
        z: !round? z : z.toFixed(2),

        a: !round? az : az.toFixed(1),
        e: !round? el : el.toFixed(1),
        s: !round? sk : sk.toFixed(1)
    };

    if (dist_xz!=null){
        result.d_xz = !round? dist_xz : dist_xz.toFixed(1);
        result.d_xyz = !round? dist_xyz : dist_xyz.toFixed(1);
    }else{
        result.d_xz = dist_xz;
        result.d_xyz = dist_xyz;
    }

    result.id = id;
    result.index = index;

    return result;

}

/**
 * Get position and orientation of the observer (=viewer=camera)
 * in the 3D scene
 */
function x3dom_getCameraPosOr(round){

    //console.log("Getting PosOr");

    var elem = Scene.element;

    var m = elem.runtime.viewMatrix().inverse();

    var tr = m.e3();

    var R0 = Data.camera.Matrices.R0;

    //var T0 = x3dom_toYawPitchRoll();
    m = R0.mult(m);

    //m = x3dom_TxMxTi(m);

    var ypr = x3dom_YawPitchRoll_nc(m);

    ypr.yaw = (180/Math.PI*ypr.yaw+360)%360;
    ypr.pitch *= 180/Math.PI;
    ypr.roll *= 180/Math.PI;

    //x3dom_matrix_test();

    if (!round){
        return {
            x: tr.x,
            y: tr.y,
            z: tr.z,
            a: ypr.yaw,
            e: ypr.pitch,
            s: ypr.roll
        };
    }else{
        return {
            x: tr.x.toFixed(2),
            y: tr.y.toFixed(2),
            z: tr.z.toFixed(2),
            a: ypr.yaw.toFixed(1),
            e: ypr.pitch.toFixed(1),
            s: ypr.roll.toFixed(1)
        };
    }

}

// this upright is for world coordinates, not the camera's
// the up vector should be taken from the initial camera orientation in kml.
function x3dom_setUpRight(){

    var mat = Scene.element.runtime.viewMatrix().inverse();

    var from = mat.e3();
    var at = from.subtract(mat.e2());
    var up = Data.camera.Matrices.Up0;

    var s = mat.e2().cross(up).normalize();

    var newup = mat.e2().cross(s).normalize().negate();

    mat = x3dom.fields.SFMatrix4f.lookAt(from, at, newup);

    x3dom_setViewpoint(mat);

}

/*
 * rotation by delta angle around camera's current Up vector
 */
function x3dom_rotation(dangle){

    var mat = Scene.element.runtime.viewMatrix();

    mat = mat.inverse();

    var from = mat.e3();
    var at = from.subtract(mat.e2());
    var up = mat.e1();

    var q0 = x3dom.fields.Quaternion.axisAngle(up, -dangle);
    var m0 = q0.toMatrix();

    var m1  = x3dom.fields.SFMatrix4f.translation(from);
    var m1n = x3dom.fields.SFMatrix4f.translation(from.negate());

    var mres = m1.mult(m0).mult(m1n);

    newat = mres.multMatrixPnt(at);

    newmat = x3dom.fields.SFMatrix4f.lookAt(from, newat, up);

    x3dom_setViewpoint(newmat);

}

/*
 * translate camera in x3dom space
 */
function x3dom_translation(dx,dy,dz){

    var delta = new x3dom.fields.SFVec3f(dx,dy,dz);

    var mat = Scene.element.runtime.viewMatrix().inverse();

    var from = mat.e3();
    var at = from.subtract(mat.e2());
    var up = mat.e1();

    var newfrom = from.add(delta);
    var newat = newfrom.subtract(mat.e2());

    var newmat = x3dom.fields.SFMatrix4f.lookAt(newfrom, newat, up);

    x3dom_setViewpoint(newmat);

}

function x3dom_altelev(alt,elev){

    //x3dom_matrix_test();

    var mat = Scene.element.runtime.viewMatrix().inverse();
    var R0 = Data.camera.Matrices.R0;
    //var T = x3dom_toYawPitchRoll();
    var from = mat.e3();
    from.y = alt;


    var mat = R0.mult(mat);
    var ypr = x3dom_YawPitchRoll_nc(mat);
    var ypr2 = x3dom_YawPitchRoll_nc_degs(mat);
    //console.log("Check1");
    //console.log(ypr2);

    var az = ypr.yaw;
    var el = elev;
    var sk = ypr.roll;

    var Mh = x3dom.fields.SFMatrix4f.rotationZ(az);
    var Mt = x3dom.fields.SFMatrix4f.rotationY(el);
    var Mr = x3dom.fields.SFMatrix4f.rotationX(sk);

    var R = Mh.mult(Mt).mult(Mr);

    //var R_rw = T.inverse().mult(R).mult(T);
    var R_rw = x3dom_TixMxT(R);
    var R_w = R0.inverse().mult(R_rw);

    var ypr2 = x3dom_YawPitchRoll_nc_degs(R_rw);
    //console.log("Check2");
    //console.log(ypr2);

    var matt  = x3dom.fields.SFMatrix4f.translation(from);

    var newmat = matt.mult(R_w);

    x3dom_setViewpoint(newmat);

}

/**
 * back and forth conversions for tests
 */
function x3dom_matrix_test(){

    console.log("begin==================================");

    var viewpoint = $(Scene.element).find("Viewpoint");

    console.log("Viewpoint DOM element");
    console.log("position: "+viewpoint.attr("position"));
    console.log("orientation: "+viewpoint.attr("orientation"));

    /*
     * 1. view matrix:
     *      - from world to camera
     *          - cols - world basis in camera coords
     * 2. view matrix inverted:
     *      - from camera to world
     *          - cols - camera basis in world coords
     *      - rotation matrix by def, order is not conv:
     *          - yx'z" vs zy'x"
     */

    var mat = Scene.element.runtime.viewMatrix();

    console.log("1. View Matrix from runtime");
    console.log(mat.toString());

    var mat_i = mat.inverse();

    console.log("2. Inverted View Matrix");
    console.log(mat_i.toString());

    var from = mat_i.e3();
    var at   = from.subtract(mat_i.e2());
    var up   = mat_i.e1();

    console.log("3. From-At-Up");

    var mat_fau = x3dom.fields.SFMatrix4f.lookAt(from, at, up);

    console.log(mat_fau.toString());

    var T = x3dom_toYawPitchRoll();

    var mat_eul = T.mult(mat_i).mult(T.inverse());
    var eangles = x3dom_YawPitchRoll_degs(mat_eul);

    console.log(eangles);

    var R = mat;
    var az = Math.atan2(R._02,R._22)*180/Math.PI;
    var el = -Math.asin(R._12)*180/Math.PI;
    var sk = Math.atan2(R._10,R._11)*180/Math.PI;

    console.log("Angles:");
    console.log("az="+az+" el="+el+" sk="+sk);


    console.log("matrix from angles");

    var matx = x3dom.fields.SFMatrix4f.rotationX(el*Math.PI/180);
    var maty = x3dom.fields.SFMatrix4f.rotationY(az*Math.PI/180);
    var matz = x3dom.fields.SFMatrix4f.rotationZ(sk*Math.PI/180);

    var m1  = x3dom.fields.SFMatrix4f.translation(from);
    var m1n = x3dom.fields.SFMatrix4f.translation(from.negate());

    var newmat = maty.mult(matx).mult(matz);

    console.log(newmat.toString());

    console.log("end==================================");
}

/**
 * Transform to calculate conventional Euler angles for z-y'-x" = z-y-z
 * unrelated: what's x3dom's native getWCtoCCMatrix()? canvas-to-world?
 */

function x3dom_toYawPitchRoll(){
    return new x3dom.fields.SFMatrix4f(
        0, 0,-1, 0,
        1, 0, 0, 0,
        0,-1, 0, 0,
        0, 0, 0, 1
    );
}

/*
 * For Yaw Pitch Roll
 */
function x3dom_TixMxT(m){
    return new x3dom.fields.SFMatrix4f(
        m._11,-m._12,-m._10, m._03,
       -m._21, m._22, m._20, m._13,
       -m._01, m._02, m._00, m._23,
        m._30, m._31, m._32, m._33
    );
}

/*
 * For Yaw Pitch Roll
 */
function x3dom_TxMxTi(m){
    return new x3dom.fields.SFMatrix4f(
        m._22,-m._20, m._21, m._03,
       -m._02, m._00,-m._01, m._13,
        m._12,-m._10, m._11, m._23,
        m._30, m._31, m._32, m._33
    );
}

function x3dom_YawPitchRoll(m){

    var yaw = Math.atan2(m._10,m._00);
    var pitch = -Math.asin(m._20);
    var roll = Math.atan2(m._21,m._22);

    return {
        yaw: yaw,
        pitch: pitch,
        roll: roll
    };
}

function x3dom_YawPitchRoll_degs(m){

    var a = x3dom_YawPitchRoll(m);

    return {
        yaw: a.yaw*180/Math.PI,
        pitch: a.pitch*180/Math.PI,
        roll: a.roll*180/Math.PI
    };
}

/*
 * from not converted matrix
 */
function x3dom_YawPitchRoll_nc(m){

    var yaw = -Math.atan2(m._02,m._22);
    var pitch = -Math.asin(m._12);
    var roll = -Math.atan2(m._10,m._11);

    return {
        yaw: yaw,
        pitch: pitch,
        roll: roll
    };

}

function x3dom_YawPitchRoll_nc_degs(m){

    var a = x3dom_YawPitchRoll_nc(m);

    return {
        yaw: a.yaw*180/Math.PI,
        pitch: a.pitch*180/Math.PI,
        roll: a.roll*180/Math.PI
    };

}

/*
function x3dom_YawPitchRoll_2_degs(m){

    var pitch = Math.PI+Math.asin(m._20);
    var roll = Math.atan2(m._21/Math.cos(pitch),m._22/Math.cos(pitch));
    var yaw = Math.atan2(m._10/Math.cos(pitch),m._00/Math.cos(pitch));

    return {
        yaw: yaw*180/Math.PI,
        pitch: pitch*180/Math.PI,
        roll: roll*180/Math.PI
    };
}
*/
function x3dom_delta_map2scene(p0,p1){

    var pi = new L.LatLng(p0.lat,p1.lng);

    var dx = p0.distanceTo(pi);
    var dy = 0;
    var dz = p1.distanceTo(pi);

    var dp_rw = new x3dom.fields.SFVec3f(dx,dy,dz);

    if (p1.lng<p0.lng) dp_rw.x = -dp_rw.x;
    if (p1.lat>p0.lat) dp_rw.z = -dp_rw.z;

    var M0 = Data.camera.Matrices.R0.inverse();
    var dp_w = M0.multMatrixVec(dp_rw);

    return dp_w;

}

// x,y,z - x3dom internal coords
function x3dom_getDistAngle(x,y,z){

    var R0 = Data.camera.Matrices.R0;
    var p_w = new x3dom.fields.SFVec3f(x,y,z);
    var p_rw = R0.multMatrixVec(p_w);

    var d = Math.sqrt(p_rw.x*p_rw.x+p_rw.z*p_rw.z);
    var a = Math.atan2(p_rw.x,-p_rw.z)*180/Math.PI;

    return Array(d,a);

}

function x3dom_update_map(){

    var Camera = Map.marker;

    // real world ypr from viewmatrix

    var mat = Scene.element.runtime.viewMatrix().inverse();
    var R0 = Data.camera.Matrices.R0;
    //var T = x3dom_toYawPitchRoll();

    var p_w = mat.e3();
    var dp_w = mat.e3();

    //var m_rw = T.mult(R0).mult(mat).mult(T.inverse());

    // R0 - rw -> w
    mat = R0.mult(mat);

    var ypr = x3dom_YawPitchRoll_nc_degs(mat);

    var heading = ypr.yaw;

    Map.marker.setHeading(heading);

    // real world angle distance of some point

    if (Scene.old_view_translation != null){
        dp_w = dp_w.subtract(Scene.old_view_translation);
    }

    // from w to rw
    dp_rw = R0.multMatrixVec(dp_w);

    var distance = Math.sqrt(dp_rw.x*dp_rw.x+dp_rw.z*dp_rw.z);

    var angle = 0;

    if (dp_rw.z!=0){
        angle = 180/Math.PI*Math.atan2(dp_rw.x,-dp_rw.z);
    }

    //angle = angle + heading;

    var initial_coordinates = [Data.camera.latitude,Data.camera.longitude];

    var p0 = new L.LatLng(initial_coordinates[0],initial_coordinates[1]);//Camera._latlng;
    var p1 = p0.CoordinatesOf(angle,distance);

    Map.marker.setBasePoint(p1);
    Map.marker._syncMeasureMarkersToBasePoint();

    Data.camera.latitude = p1.lat;
    Data.camera.longitude = p1.lng;
    Data.camera.heading = heading;

    Scene.old_view_translation = p_w;

}

// uses globals
function x3dom_setViewpoint(m){

    //console.log("Setting a viewpoint");

    var Q = new x3dom.fields.Quaternion(0, 0, 1, 0);
    Q.setValue(m);
    var AA = Q.toAxisAngle();

    var viewpoint = $(Scene.element).find("Viewpoint");
    viewpoint.attr("orientation",AA[0].toString()+" "+AA[1]);
    viewpoint.attr("position",m.e3().toString());
    viewpoint.attr("centerOfRotation",m.e3().toString());

    // update every time
    Data.camera.Matrices.RC_w = m;

}