/**
 * @author Martin Karkowski
 * @email m.karkowski@zema.de
 * @create date 2018-06-20 03:09:56
 * @modify date 2018-06-24 05:58:55
 * @desc [description]
*/

import * as geometry from '../src/geometric-functions';
import * as math from 'mathjs';

console.log(geometry.QuatToRPY(geometry.rotMatrixToQuat(geometry.quatToRotMatrix(geometry.RPYtoQuat({ roll: 1, pitch: 0, yaw: 0 })))));

const _quat = {
    imgX: 0,
    imgY: 0,
    imgZ: 0,
    real: 1
};

const _rpy = {
    roll: math.pi*0.5,
    pitch: 0,
    yaw: 0
}

const _originTrans = {
    x: 0,
    y: 0,
    z: 0
};

const _transTo = {
    x: 1,
    y: 2,
    z: 3
};

console.log('TEST QUAT => MAT => QUAT', geometry.rotMatrixToQuat(geometry.quatToRotMatrix({imgX:0,imgY:-1,imgZ:0,real:Math.PI/2})));

const _origin = geometry.homogenousCoordinates(_originTrans, _quat);
const _target = geometry.homogenousCoordinates(_transTo, geometry.RPYtoQuat(_rpy));
const _transformation = geometry.createTransformation(_origin, _target);

console.log(
    'From:\n', _origin.matrix.toArray(),
    'To:\n', _target.matrix.toArray(),
    '\nTransformation:\n', _transformation.matrix.toArray(),
    '\nDestionation:\n', math.multiply(_transformation.matrix, _origin.matrix).toArray());

console.log(geometry.extractTranslation(_transformation));

console.log(geometry.extractQuaternion(_transformation));

console.log(geometry.transformPoint(_transformation, {x:1,y:1,z:1}));