$(document).ready(function() {

    const RHO = 1000;
    const PI = Math.PI;
    const g = 9.81;

    // Ship properties
    const SCALE = 27;
    const DRAFT = 2.578/SCALE;
    const LOA = 43/SCALE;
    const BEAM = 12/SCALE;
    const KEELLENGTH = 40.6/SCALE;
    const KEELBREADTH = 1/SCALE;
    const KEELHEIGHT = 0.625/SCALE;
    const BOTTOMBREADTH = 5.5/SCALE;
    const BOTTOMHEIGHT = 1.275/SCALE;

    // Wave properties
    const HEIGHT = 2.8/SCALE;
    const PERIOD = 5.8/Math.sqrt(SCALE);
    const DEPTH = 3;
    const LAMBDA = getWavelength(PERIOD, DEPTH);
    const WAVETYPE = checkDepth(HEIGHT, LAMBDA);
    const ZETA = HEIGHT*0.5;
    const WAVENUM = 2*PI/LAMBDA;
    const OMEGA = 2*PI*PERIOD;

    const FOV = 45;
    const ASPECT = window.innerWidth/window.innerHeight;
    const NEAR = 0.1;
    const FAR = 2000;

    container = document.createElement( 'div' );
    document.body.appendChild( container );

    var renderer = new THREE.WebGLRenderer({ antialias: true });
    renderer.setPixelRatio( window.devicePixelRatio );
    renderer.setSize( window.innerWidth,window.innerHeight );
    renderer.setClearColor( 0xffffff, 1);
    container.appendChild( renderer.domElement );

    var scene = new THREE.Scene();

    var camera = new THREE.PerspectiveCamera( FOV,ASPECT,NEAR,FAR );
    camera.position.set( 60/SCALE , 20/SCALE , 10/SCALE );
    camera.up.set( 0,0,1 );
    scene.add( camera );

    var controls = new THREE.OrbitControls( camera, renderer.domElement );
    controls.enablePan = true;
    controls.minDistance = 1/SCALE;
    controls.maxDistance = 2000/SCALE;
    controls.target.set( 0, 0, 3/SCALE );

    // Initialize GUI
    var gui = new dat.GUI({
        height : 5*32 - 1
    });
    var guiParams = {
        RPM : 10000,
        gimbalAMP : 0,
        gimbalPeriod : 1,
        waves : false
    };
    gui.add( guiParams, 'RPM', 0, 12000).name('Disk RPM');
    gui.add( guiParams, 'gimbalAMP', 0, 90).name('Nutation AMP');
    gui.add( guiParams, 'gimbalPeriod', 0.2, 5).name('Nutation period');
    gui.add( guiParams, 'waves');

    // Load model
    var ship = new THREE.Object3D();
    var mtlLoader = new THREE.MTLLoader();
    mtlLoader.load( 'models/baat.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/baat.obj', function ( object ) {
            ship.add( object );
        });
    });
    var gyroCribPS = new THREE.Object3D();
    mtlLoader = new THREE.MTLLoader();
    mtlLoader.load( 'models/crib.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/crib.obj', function ( object ) {
            gyroCribPS.add( object );
        });
    });
    var gyroCribSB = new THREE.Object3D();
    mtlLoader = new THREE.MTLLoader();
    mtlLoader.load( 'models/crib.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/crib.obj', function ( object ) {
            gyroCribSB.add( object );
        });
    });
    var pinionPS = new THREE.Object3D();
    mtlLoader = new THREE.MTLLoader();
    mtlLoader.load( 'models/pinion.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/pinion.obj', function ( object ) {
            pinionPS.add( object );
        });
    });
    var pinionSB = new THREE.Object3D();
    mtlLoader = new THREE.MTLLoader();
    mtlLoader.load( 'models/pinion.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/pinion.obj', function ( object ) {
            pinionSB.add( object );
        });
    });
    var gimbalPS = new THREE.Object3D();
    mtlLoader = new THREE.MTLLoader();
    mtlLoader.load( 'models/gimbal.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/gimbal.obj', function ( object ) {
            gimbalPS.add( object );
        });
    });
    var gimbalSB = new THREE.Object3D();
    mtlLoader = new THREE.MTLLoader();
    mtlLoader.load( 'models/gimbal.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/gimbal.obj', function ( object ) {
            gimbalSB.add( object );
        });
    });
    var flywheelPS = new THREE.Object3D();
    mtlLoader.load( 'models/flywheel.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/flywheel.obj', function ( object ) {
            flywheelPS.add( object );
        });
    });
    var flywheelSB = new THREE.Object3D();
    mtlLoader.load( 'models/flywheel.mtl', function( materials ) {
        materials.preload();
        var objLoader = new THREE.OBJLoader();
        objLoader.setMaterials( materials );
        objLoader.load( 'models/flywheel.obj', function ( object ) {
            flywheelSB.add( object );
        });
    });

    gyroCribPS.position.y = 2/SCALE;
    gyroCribPS.position.z = 5.223/SCALE;
    gyroCribPS.rotation.z = Math.PI;

    gyroCribSB.position.y = -gyroCribPS.position.y;
    gyroCribSB.position.z = gyroCribPS.position.z;

    gimbalPS.position.z = 2.16/SCALE;
    gimbalSB.position.z = gimbalPS.position.z;

    pinionPS.position.z = 0.795/SCALE;
    pinionSB.position.z = pinionPS.position.z;

    gimbalPS.add( flywheelPS );
    gimbalSB.add( flywheelSB );
    gyroCribPS.add( gimbalPS );
    gyroCribSB.add( gimbalSB );
    gyroCribPS.add( pinionPS );
    gyroCribSB.add( pinionSB );
    ship.add( gyroCribPS );
    ship.add( gyroCribSB );

    var COF = new THREE.Object3D();
    ship.position.z = -DRAFT;
    COF.add( ship );
    scene.add( COF );

    // Axis
    var material = new THREE.LineBasicMaterial({
        color: 0x0000ff
    });
    var linegeo = new THREE.Geometry();
    linegeo.vertices.push(
        new THREE.Vector3( 25/SCALE, 0, 0 ),
        new THREE.Vector3( 0, 0, 0 ),
        new THREE.Vector3( 0, 25/SCALE, 0 ),
        new THREE.Vector3( 0, 0, 0 ),
        new THREE.Vector3( 0, 0, 25/SCALE )
    );
    var line = new THREE.Line( linegeo, material );

    var origin = new THREE.Object3D();
    origin.add( line );
    origin.add( COF );
    scene.add( origin );

    var ambientLight = new THREE.AmbientLight(0xffffff, 0.4);
    scene.add(ambientLight);
    var light1 = new THREE.DirectionalLight( 0xffffbb, 1 );
    light1.position.set( 1, 1, 1 );
    scene.add( light1 );
    var light2 = new THREE.DirectionalLight( 0xffffbb, 1 );
    light2.position.set( -1, -1, -1 );
    scene.add( light2 );
    var light3 = new THREE.DirectionalLight( 0xffffbb, 1 );
    light3.position.set( 1, -1, 1 );
    //scene.add( light3 );

    // Waves
    var geometry, points = [];
    var width  = 81/SCALE;
    var height = 200/SCALE;
    var widthSegments = 200;
    var heightSegments = 200;

    geometry = new THREE.PlaneGeometry( width, height, widthSegments, heightSegments );
    var oceanmat = new THREE.MeshLambertMaterial( {
        color: 0xccffff,
        opacity: 0.5,
        transparent: true,
        side: THREE.DoubleSide
    } );
    var ocean = new THREE.Mesh( geometry, oceanmat );
    scene.add( ocean );

    var t = 0;
    var dt = 0.01;
    var w = [0,0,0];
    var q = [0,0,0,0,0,0];
    var phi = 0;
    var psi = 0;
    var rotMat = eyeMat(3,3);
    var shipData, roll, pitch, yaw, rolld, rollvel, dpsi, RPM, phiPin, dx;

    var render = function () {
        requestAnimationFrame(render);
        var time = performance.now() * 0.001;

        if ( true ){
            RPM = guiParams.RPM;
            dpsi = RPM/60*2*Math.PI;
            psi += dpsi*dt;

            shipData = shipMotions(q,rotMat,t,dt);
            dx = shipData.dx;
            w = shipData.w;
            rotMat = shipData.rotMat;
            roll = shipData.shipAngle[0];
            pitch = shipData.shipAngle[1];
            yaw = shipData.shipAngle[2];
            phi = shipData.phi;

            phiPin = -0.6*1.85*phi;

            rolld = roll*180/Math.PI;
            rollvel = w[0]*180/Math.PI;

            ship.position.x += dx[0]*dt;
            ship.position.y += dx[1]*dt;
            ship.position.z += dx[2]*dt;
            COF.rotation.x = roll;
            COF.rotation.y = pitch;
            COF.rotation.z = yaw;
            gimbalPS.rotation.y = phi;
            gimbalSB.rotation.y = phi;
            pinionPS.rotation.y = phiPin;
            pinionSB.rotation.y = phiPin;
            flywheelPS.rotation.z = psi;
            flywheelSB.rotation.z = psi;

            q = [ dx[0], dx[1], dx[2], w[0], w[1], w[2]];
            t += dt;
        }

        updatePoints(t);
        controls.update();
        renderer.render(scene, camera);
    };
    render();

    function updatePoints(t) {
        for(var i = 0; i < ocean.geometry.vertices.length; i++) {
            var v = ocean.geometry.vertices[i];
            var y = v.y;
            if ( guiParams.waves ) {
                v.z = waveProfile(ZETA, OMEGA, WAVENUM, t, y);
            } else {
                v.z = 0;
            }
            ocean.geometry.verticesNeedUpdate = true;
        }
    }

    function shipMotions(q,rotMat,t,dt) {

        q = RK4(t,q,dt);

        var dx = [ q[0], q[1], q[2]];
        var w = [ q[3], q[4], q[5]];

        rotMat = getRotMat(rotMat,w,dt);

        var shipAngle = rotMatToAngle(rotMat);

        var gimbalData = getNutation(t);
        var phi = gimbalData.phi;

        return {
            dx : dx,
            rotMat : rotMat,
            w : w,
            shipAngle : shipAngle,
            phi : phi
        };
    }


    function RK4(x,y,h) {

        var arr = [], k = [], l = [], m = [], n = [];

        for (var i = 0; i < y.length; i++) {
            k[i] = h*myFunction( i, x, y);
        }

        for ( i = 0; i < y.length; i++) {
            arr = constMatMult(0.5,k);
            l[i] = h*myFunction( i, x, MatMatAdd(arr,y));
        }

        for ( i = 0; i < y.length; i++) {
            arr = constMatMult(0.5,l);
            m[i] = h*myFunction( i, x, MatMatAdd(arr,y));
        }

        for ( i = 0; i < y.length; i++) {
            n[i] = h*myFunction( i, x, MatMatAdd(m,y));
        }

        for ( i = 0; i < y.length; i++) {
            y[i] = y[i] + (k[i] + 2*l[i] + 2*m[i] + n[i])/6;
        }

        return y;
    }

    function myFunction( flag, t, dq) {

        var w11 = dq[3];
        var w12 = dq[4];
        var w13 = dq[5];

        const m1 = 318000/SCALE/SCALE/SCALE;
        const m4 = 0.0341;
        const m5 = 0.1453;
        const DELTA = m1 + 2*m4 + 2*m5;
        const J11 = 0.649579;
        const J12 = 3.549014;
        const J13 = 3.590727;
        const J42 = 100.32*Math.pow(10,-6);
        const J53 = 74.44*Math.pow(10,-6);

        var phi4Data = getNutation(t);
        var phi4 = phi4Data.phi;
        var dphi4 = phi4Data.dphi;

        var RPM = guiParams.RPM;
        var dpsi5 = RPM/60*2*Math.PI;

        // Wave forces and moments
        var Fw1 = 0;
        var Fw2 = 0;
        var Fw3 = 0;
        var Mw1 = 0;
        var Mw2 = 0;
        var Mw3 = 0;
        if ( guiParams.waves ) {
            Mw1 = 50 * ZETA * Math.sin(2 * PI / PERIOD * t);
        }

        // Forces
        var Fg = -DELTA*g;
        var Fb = -Fg;
        var F2 = Fw2;
        var F3 = Fg + Fb + Fw3;

        // Rightning Moment
        var GMT = 9.85/SCALE;
        var GML = 80.513/SCALE;
        var Mb1 = Fb*calculateRightingArm(GMT, COF.rotation.x);
        var Mb2 = -Fb*calculateRightingArm(GML, COF.rotation.y);

        // Damping
        var Broll = 2;
        var Bpitch = 3;
        var Byaw = 3;
        var Md1 = -Broll*w11;
        var Md2 = -Bpitch*w12;
        var Md3 = -Byaw*w13;

        // Total moments
        var M1 = Mw1 + Mb1 + Md1;
        var M2 = Mw2 + Mb2 + Md2;
        var M3 = Mw3 + Md3;

        var ddq;
        if (flag == 0) {
            ddq = Fw1/(m1 + 2*m4 + 2*m5);
        }
        if (flag == 1) {
            ddq = F2/(m1 + 2*m4 + 2*m5);
        }
        if (flag == 2) {
            ddq = F3/(m1 + 2*m4 + 2*m5);
        }
        if (flag == 3) {
            ddq = M1;
            ddq -= J13*w12*w13 - J12*w12*w13 + J53*w12*w13*Math.cos(phi4)*Math.cos(phi4) + 2*J53*Math.cos(phi4)*dphi4*dpsi5 + J53*w11*Math.sin(2*phi4)*dphi4;
            ddq /= J11 + 2*J42 + J53 + J53*Math.sin(phi4)*Math.sin(phi4);
        }
        if (flag == 4) {
            ddq = M2;
            ddq -= J11*w11*w13 - J13*w11*w13 - J53*w11*w13 + 2*J53*w11*w13*Math.sin(phi4)*Math.sin(phi4) + 2*J53*w13*Math.sin(phi4)*dpsi5;
            ddq /= J12 + 2*J42 + J53;
        }
        if (flag == 5) {
            ddq = M3;
            ddq -= J12*w11*w12 - J11*w11*w12 - J53*w11*w12 + J53*w11*w12*Math.cos(phi4)*Math.cos(phi4) - 2*J53*w12*Math.sin(phi4)*dpsi5 - 2*J53*w13*Math.cos(phi4)*Math.sin(phi4)*dphi4;
            ddq /= J13 + 2*J42 + 2*J53 - J53*Math.sin(phi4)*Math.sin(phi4);
        }
        return ddq;
    }

    function getNutation( t ) {
        var Ag = (PI/180)*guiParams.gimbalAMP;
        var Tg = guiParams.gimbalPeriod;
        var phi = Ag*Math.sin(2*Math.PI/Tg*t);
        var dphi = 2*Math.PI/Tg*Ag*Math.cos(2*Math.PI/Tg*t);
        return {
            phi: phi,
            dphi: dphi
        };
    }
});