// JavaScript source code
var AMP1 = 0;
var AMP2 = 0;
var PER1 = period(10);
var PER2 = period(3);
var x = 0;
var y = 0;
const phase = Math.PI/2;

const ROV_length = 1.0;
const V_length = .5;
const arm_length = .5;
const m1 = 2000; 
const m3 = 300;
const J11 = ROV_length * ROV_length * m1 / 6;
const J31 = m3 * arm_length * arm_length / 3;
const geo =  0.5;

var rotationMatrix = eyeMat(3, 3);
var identityMat = eyeMat(3, 3);
function getTheta(t) {

    return {
        th1t: AMP1 * Math.sin(t * PER1 + phase + x),
        dth1t: AMP1 * PER1 * Math.cos(t * PER1 + phase),
        ddth1t: -AMP1 * PER1 * PER1 * Math.sin(t * PER1 +phase),
        th2t: AMP2 * Math.sin(t * PER2 + phase+y),
        dth2t: AMP2 * PER2 * Math.cos(t * PER2 + phase),
        ddth2t: -AMP2 * PER2 * PER2 * Math.sin(t * PER2 +phase)
    }
}

function matlabFunction(om_flag, prev_dq, t,q) {





    var w1 = prev_dq[0];
    var w2 = prev_dq[1];
    var w3 = prev_dq[2];
    var theta = getTheta(t);


    var th1 = theta.th1t;
    var dth1 = theta.dth1t;
    var ddth1 = theta.ddth1t;
    var th2 = theta.th2t;
    var dth2 = theta.dth2t;
    var ddth2 = theta.ddth2t;
    var M = getMoments(q,m1,prev_dq);
    const M1x = M.M1x;
    const M1y = M.M1y;

    switch (om_flag) {
        case 0:
            return (J11*J31*ddth2*Math.cos(th1) - J31*M1x*Math.cos(th2)**2 - J11*M1x + J31*M1x*Math.cos(th1)**2*Math.cos(th2)**2 - J11*J31*w2*w3 + J11*J31*dth2*w1*Math.sin(2*th2) + J11*J31*w2*w3*Math.cos(th2)**2 - 2*J11*J31*dth1*dth2*Math.sin(th1) - 2*J11*J31*dth2*w3*Math.sin(th1) + J31*M1y*Math.cos(th1)*Math.cos(th2)**2*Math.sin(th1) + 2*J11*J31*dth1*w2*Math.cos(th1)**2*Math.cos(th2)**2 + J11*J31*dth1**2*Math.cos(th1)*Math.cos(th2)*Math.sin(th2) + J11*J31*w2*w3*Math.cos(th1)**2*Math.cos(th2)**2 - J11*J31*w2**2*Math.cos(th1)*Math.cos(th2)*Math.sin(th2) + J11*J31*w3**2*Math.cos(th1)*Math.cos(th2)*Math.sin(th2) + 2*J11*J31*dth1*dth2*Math.cos(th2)**2*Math.sin(th1) + 2*J11*J31*dth2*w3*Math.cos(th2)**2*Math.sin(th1) + J11*J31*ddth1*Math.cos(th2)*Math.sin(th1)*Math.sin(th2) + 2*J11*J31*dth1*w3*Math.cos(th1)*Math.cos(th2)*Math.sin(th2) + J11*J31*w1*w2*Math.cos(th2)*Math.sin(th1)*Math.sin(th2) - 2*J11*J31*dth1*w1*Math.cos(th1)*Math.cos(th2)**2*Math.sin(th1) - 2*J11*J31*dth2*w1*Math.cos(th1)**2*Math.cos(th2)*Math.sin(th2) - J11*J31*w1*w3*Math.cos(th1)*Math.cos(th2)**2*Math.sin(th1) - 2*J11*J31*dth2*w2*Math.cos(th1)*Math.cos(th2)*Math.sin(th1)*Math.sin(th2))/(J11*(J11 + J31));
        case 1:
            return  (J11*J31*ddth2*Math.sin(th1) - J11*M1y - J31*M1y*Math.cos(th1)**2*Math.cos(th2)**2 - 2*J11*J31*dth1*w1*Math.cos(th2)**2 + 2*J11*J31*dth1*dth2*Math.cos(th1) + 2*J11*J31*dth2*w3*Math.cos(th1) - J11*J31*w1*w3*(2*Math.cos(th2)**2 - 1) + J31*M1x*Math.cos(th1)*Math.cos(th2)**2*Math.sin(th1) + 2*J11*J31*dth1*w1*Math.cos(th1)**2*Math.cos(th2)**2 + J11*J31*w1*w3*Math.cos(th1)**2*Math.cos(th2)**2 + J11*J31*dth1**2*Math.cos(th2)*Math.sin(th1)*Math.sin(th2) - J11*J31*w1**2*Math.cos(th2)*Math.sin(th1)*Math.sin(th2) + J11*J31*w3**2*Math.cos(th2)*Math.sin(th1)*Math.sin(th2) - 2*J11*J31*dth1*dth2*Math.cos(th1)*Math.cos(th2)**2 - 2*J11*J31*dth2*w3*Math.cos(th1)*Math.cos(th2)**2 - J11*J31*ddth1*Math.cos(th1)*Math.cos(th2)*Math.sin(th2) + 2*J11*J31*dth1*w3*Math.cos(th2)*Math.sin(th1)*Math.sin(th2) + J11*J31*w1*w2*Math.cos(th1)*Math.cos(th2)*Math.sin(th2) + 2*J11*J31*dth1*w2*Math.cos(th1)*Math.cos(th2)**2*Math.sin(th1) + 2*J11*J31*dth2*w2*Math.cos(th1)**2*Math.cos(th2)*Math.sin(th2) + J11*J31*w2*w3*Math.cos(th1)*Math.cos(th2)**2*Math.sin(th1) - 2*J11*J31*dth2*w1*Math.cos(th1)*Math.cos(th2)*Math.sin(th1)*Math.sin(th2))/(J11*(J11 + J31));
        case 2:
            return - (J31*Math.cos(th2)*(2*dth1*dth2*Math.sin(th2) - ddth1*Math.cos(th2) + 2*dth2*w3*Math.sin(th2) + w1*w3*Math.cos(th1)*Math.sin(th2) + w1*w2*Math.cos(th2)*(2*Math.cos(th1)**2 - 1) + w2*w3*Math.sin(th1)*Math.sin(th2) - w1**2*Math.cos(th1)*Math.cos(th2)*Math.sin(th1) + w2**2*Math.cos(th1)*Math.cos(th2)*Math.sin(th1) + 2*dth2*w2*Math.cos(th1)*Math.cos(th2) - 2*dth2*w1*Math.cos(th2)*Math.sin(th1)))/(J11 + J31) - (J31*Math.cos(th2)*(M1y*Math.cos(th1)*Math.sin(th2) - M1x*Math.sin(th1)*Math.sin(th2)))/(J11*(J11 + J31));
    }
    console.log("ERROR in Matlab-Function, swtich");
    return 0;
}

function getMoments(q,m1,dq){

    const length = 0.5;
    const gravity = 9.81;
    const mass = m1;
    return{
        M1x :   - m1 * length * gravity * Math.sin(q[0]),
        M1y :    m1 * length * gravity * Math.sin(q[1])
    }
}
function updateROV(dq, dt, t,q) {
    if(period(guiParams.PER1)!=PER1){
        x = Math.asin(getTheta(t).th1t/AMP1)-t*period(guiParams.PER1)-phase;
        while(x > 2 * Math.PI){
            x -= (2 * Math.PI);
        }
        while(x < 0){
            x += 2 * Math.PI;
        }
        PER1 = period(guiParams.PER1);
    }
    if(period(guiParams.PER2)!=PER2){
        y = Math.asin(getTheta(t).th2t/AMP2)-t*period(guiParams.PER2)-phase;
        while(y > 2 * Math.PI){
            y -= (2 * Math.PI);
        }
        while(y < 0){
            y += 2 * Math.PI;
        }
        PER2 = period(guiParams.PER2);
    }
    AMP1 = AMP1 > guiParams.AMP1 + 0.05 ? AMP1 - 0.005 : AMP1 < guiParams.AMP1 - 0.05 ? AMP1 + 0.005 : AMP1;
    AMP2 = AMP2 > guiParams.AMP2 + 0.05 ? AMP2 - 0.005 : AMP2 < guiParams.AMP2 - 0.05 ? AMP2 + 0.005 : AMP2;
    var ddq = RK4(dq, dt, t,q);
    rotationMatrix = getRotMat(rotationMatrix, ddq, dt);
    var angles = rotMatToAngle(rotationMatrix);
    return {
        angles: angles,
        ddq: ddq
    }

}


function RK4(last_dq, dt, t,q) {
    var k1 = [], k2 = [], k3 = [], k4 = [];
    var arr = [];
    var dq = last_dq.slice();
    for (var a = 0 ; a < dq.length ; a++) {
        k1[a] = dt * matlabFunction(a, dq, t,q);
    }
    for (var a = 0 ; a < dq.length ; a++) {
        for (var i = 0 ; i < k1.length ; arr[i] = dq[i] + k1[i] / 2, i++);
        k2[a] = dt * matlabFunction(a, arr, t + dt / 2,q)
    }
    for (var a = 0 ; a < dq.length ; a++) {
        for (var i = 0 ; i < k2.length ; arr[i] = dq[i] + k2[i] / 2, i++);
        k3[a] = dt * matlabFunction(a, arr, t + dt / 2,q);
    }
    for (var a = 0 ; a < dq.length ; a++) {
        for (var i = 0 ; i < k3.length ; arr[i] = dq[i] + k3[i], i++);
        k4[a] = dt * matlabFunction(a, arr, t + dt,q);
    }
    for (var i = 0 ; i < dq.length ; dq[i] = dq[i] + (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) / 6, i++);

    return dq;
}

function rotMatToAngle(rotMat) {
    var r00 = rotMat[0][0];
    var r01 = rotMat[0][1];
    var r10 = rotMat[1][0];
    var r11 = rotMat[1][1];
    var r02 = rotMat[0][2];
    var r20 = rotMat[2][0];
    var r12 = rotMat[1][2];
    var r21 = rotMat[2][1];
    var r22 = rotMat[2][2];

    var rot1 = Math.atan2(r12, r22);
    var s1 = Math.sin(rot1);
    var c1 = Math.cos(rot1);
    var c2 = Math.sqrt(r00 * r00 + r01 * r01);
    var rot2 = Math.atan2(r02, c2);
    var rot3 = Math.atan2(s1 * r20 - c1 * r10, c1 * r11 - s1 * r21);
    return [rot1, rot2, rot3];
}
 
function eyeMat(rows, cols) {
    var Mat = zerosMat([rows, cols]);
    for (var i = 0; i < rows; i++) {
        Mat[i][i] = 1;
    }
    return Mat;
}

function getRotMat(rotMat, w, dt) {
    var w1 = w[0];
    var w2 = w[1];
    var w3 = w[2];
    var wNorm = Math.sqrt(w1 * w1 + w2 * w2 + w3 * w3);
    if (w1 * w2 * w3 < 0) wNorm = -wNorm;

    var wSkew = [[0, -w3, w2], [w3, 0, -w1], [-w2, w1, 0]];

    var c2 = Math.sin(dt * wNorm) / wNorm;
    var c3 = (1 - Math.cos(dt * wNorm)) / (wNorm * wNorm);

    var term1 = eyeMat(3, 3);
    var term2 = constMatMult(c2, wSkew);
    var term3 = constMatMult(c3, MatMatMult(wSkew, wSkew));

    var expwt = MatMatAdd(term1, MatMatAdd(term2, term3));
    rotMat = MatMatMult(rotMat, expwt);

    return rotMat;
}

function constMatMult(c, Mat) {
    var rows = Mat.length;
    var cols = Mat[0].length;
    for (var i = 0; i < rows; i++) {
        for (var j = 0; j < cols; j++) {
            Mat[i][j] = c * Mat[i][j];
        }
    }
    return Mat;
}

function MatMatAdd(Mat1, Mat2) {
    var rows = Mat1.length;
    var cols = Mat1[0].length;
    for (var i = 0; i < rows; i++) {
        for (var j = 0; j < cols; j++) {
            Mat1[i][j] += Mat2[i][j];
        }
    }
    return Mat1;
}

function MatMatMult(Mat1, Mat2) {
    var rows1 = Mat1.length;
    var cols1 = Mat1[0].length;
    var rows2 = Mat2.length;
    var cols2 = Mat2[0].length;
    var Mat = zerosMat([rows1, cols2]);

    for (var i = 0; i < rows1; i++) {
        for (var j = 0; j < cols2; j++) {
            for (var k = 0; k < cols1; k++) {
                Mat[i][j] += Mat1[i][k] * Mat2[k][j];
            }
        }
    }
    return Mat;
}

function zerosMat(dimensions) {
    var mat = [];
    for (var i = 0; i < dimensions[0]; ++i) {
        mat.push(dimensions.length == 1 ? 0 : zerosMat(dimensions.slice(1)));
    }
    return mat;
}

function period(seconds) {
    return (Math.PI * 2) / seconds;
}