var width = document.getElementById("viewport").offsetWidth
var height = 300;

var renderer = new THREE.WebGLRenderer({ antialias: true });
renderer.setSize(width, height);
document.getElementById("viewport").appendChild(renderer.domElement);

var scene = new THREE.Scene();

var cubeGeometry = new THREE.CubeGeometry(100, 100/2, 100*1.5);
var cubeGeometry2 = new THREE.CubeGeometry(120, 120/2, 120*1.5);
// var cubeMaterial = new THREE.MeshLambertMaterial({ color: 0xffffff });
const color = new THREE.Color("rgb(255, 0, 0)");
var cubeMaterial = new THREE.MeshPhongMaterial({
    color: color,
    opacity: 1,
    transparent: true,
});
const color2 = new THREE.Color("rgb(48,117,255)");
var cubeMaterial2 = new THREE.MeshPhongMaterial({
    color: color2,
    opacity: 0.5,
    transparent: true,
});
var cube = new THREE.Mesh(cubeGeometry, cubeMaterial);
var cube2 = new THREE.Mesh(cubeGeometry2, cubeMaterial2);
// cube.rotation.y = Math.PI * 45 / 180;
// cube2.rotation.y = Math.PI * 45 / 180;
scene.add(cube);
scene.add(cube2);

var camera = new THREE.PerspectiveCamera(45, width / height, 0.1, 6000);
camera.position.y = 100;
camera.position.z = 240;
camera.lookAt(cube.position);

scene.add(camera);

var skyboxGeometry = new THREE.CubeGeometry(5000, 5000, 5000);
var skyboxMaterial = new THREE.MeshBasicMaterial({ color: 0x232323, side: THREE.BackSide });
var skybox = new THREE.Mesh(skyboxGeometry, skyboxMaterial);

scene.add(skybox);

const colorl = 0xFFFFFF;
const intensity = 1;
const light = new THREE.DirectionalLight(colorl, intensity);

var pointLight = new THREE.PointLight(0xffffff);
pointLight.position.set(0, 300, 200);
light.position.set(0, 300, 200);

scene.add(pointLight);

cube.position.x = 0
cube2.position.x = 0

let manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' )
let calibrationRot = new THREE.Quaternion()

function renderTCP(x, y, z) {
    let calibration = new THREE.Quaternion().setFromEuler(manCalibration)
    // let calibration = new THREE.Quaternion().setFromEuler(new THREE.Euler( radFromAngle, 0, 0, 'YXZ' ))
    let eul = new THREE.Euler( x, z, y, 'YXZ' );

    // let eul = new THREE.Euler( x, z, y, 'ZXY' ); // XYZ XZY YZX YXZ ZXY ZYX

    cube2.quaternion.setFromEuler(eul).multiply(calibrationRot)//.multiply(calibration)

    renderer.render(scene, camera);
}

function renderSerial(x, y, z) {
    // requestAnimationFrame(renderSerial);

    // let eul = new THREE.Euler( x, y, z, 'YXZ' ); // XYZ XZY YZX YXZ ZXY ZYX
    // let eul = new THREE.Euler( x, z, y, 'ZXY' ); // XYZ XZY YZX YXZ ZXY ZYX
    let eul = new THREE.Euler( x, z, y, 'YXZ' );

    cube.quaternion.setFromEuler(eul)

    renderer.render(scene, camera);
}

renderTCP(0, 0, 0);
renderSerial(0, 0, 0);

var pitchRange = document.getElementById("pitchRange");
var yawRange = document.getElementById("yawRange");
var rollRange = document.getElementById("rollRange");

pitchRange.oninput = () => {
    manCalibration.x = pitchRange.value * Math.PI / 180
    // let rot = new THREE.Euler().setFromQuaternion( cube2.quaternion, 'YXZ' );
    // renderTCP(rot.x, rot.y, rot.z)
}
yawRange.oninput = () => {
    manCalibration.y = yawRange.value * Math.PI / 180
}
rollRange.oninput = () => {
    manCalibration.z = rollRange.value * Math.PI / 180
}

function delCalibration(evt) {
    calibrationRot = new THREE.Quaternion()
    manCalibration = new THREE.Euler( 0, 0, 0, 'XYZ' )
    pitchRange.value = 0
    yawRange.value = 0
    rollRange.value = 0
}

var calState = false;
function manualCalibration(evt) {
    let con = document.getElementById("manCalContainer")
    console.log("mancal", con.style.display)
    if (calState === false) {
        calState = !calState
        console.log("mancal ON")
        con.style.display = "block"
        delCalibration()
    } else {
        calState = !calState
        console.log("mancal OFF")
        con.style.display = "none"
        manCalibration = new THREE.Euler( 0, 0, 0, 'YXZ' )
        pitchRange.value = 0
        yawRange.value = 0
        rollRange.value = 0

    }
}
function calibrate(evt) {
    let serOrientation = cube.quaternion.clone()
    let tcpOrientation = cube2.quaternion.clone().multiply(calibrationRot.clone().invert())
    let diff = tcpOrientation.invert().multiply(serOrientation)
    calibrationRot = diff
};
document.getElementById("deleteCalibration").onclick = delCalibration
document.getElementById("manualCalibration").onclick = manualCalibration
document.getElementById("calibrate").onclick = calibrate