/**
 * @author Martin Karkowski
 * @email m.karkowski@zema.de
 * @create date 2018-06-15 12:11:22
 * @modify date 2020-02-28 16:14:44
 * @desc [description]
*/

/** Clear the Screen */


import 'reflect-metadata';

import * as CS from '../assembly/manual-assembly';
import * as geometry from '../src/geometric-functions';

import { Builder } from '../../ZISS-Assembly-Builder/src/Container-Builder.FileLoader';
import { rgetattr, rsetattr } from '../../ZISS-TypeScript-Library/src/Object-Methods';


function compare(orignal,adapted){

    const v1 = geometry.transformationToPose(orignal);
    const v2 = geometry.transformationToPose(adapted);

    const path = [
        'trans.x',
        'trans.y',
        'trans.z',
        'orientation.imgX',
        'orientation.imgY',
        'orientation.imgZ',
        'orientation.real',
    ]

    const _comp = {};

    for (const p of path){

        const _new = rgetattr(v1, p);
        const _org = rgetattr(v2, p);

        rsetattr(_comp, p, {
            old: _org,
            new: _new,
            diff: _org-_new
        });

    }

    
    console.log('DIFFERENCE:\n',_comp);
}

Builder.load();

Builder.on('loaded', () => {

    const _cs = Builder.instance.container.get<CS.CoordinateContainer>(CS.TYPES.CoordinateContainer);

    /** Add Root CS */
    _cs.addCoordinateSystem('root',null);

    _cs.addCoordinateSystem('robot','root',geometry.homogenousCoordinates({x:0,y:0,z:0}),'fromParent');

    _cs.addCoordinateSystem('tcp', 'robot', geometry.homogenousCoordinates(
        {
            "x": 0.1639767602561366,
            "y": -0.8244141868788568,
            "z": 0.09691757147957357
        },
        {
            "real": 0.4978925975514955,
            "imgX": 0.5034369823164051,
            "imgY": -0.5020103892132208,
            "imgZ": 0.49662836735487315
        }
    ), 'toParent');

    _cs.addCoordinateSystem('camera', 'tcp', geometry.homogenousCoordinates(
        {
            x: 0.3822017,
            y: 0.0021587,
            z: 0.022724
        },
        {
            imgX: 0,
            imgY: -1,
            imgZ: 0,
            real: 1.570796
        }
    ), 'toParent');

    _cs.addCoordinateSystem('tool', 'tcp', geometry.homogenousCoordinates(
        {
            x: 0.3822017,
            y: 0.002144,
            z: 0.0932456
        }, {
            imgX: 0,
            imgY: -1,
            imgZ: 0,
            real: 1.570796
        }
    ), 'toParent');

    const _start = _cs.transform('robot','tcp',geometry.poseToTransformation({
        orientation: geometry.UNIT_QUAT,
        trans: {x:0,y:0,z:0.0}
    }));

    console.log('ORIGINAL:\n',geometry.transformationToPose(_start));

    const res = _cs.multiTransform({src: 'camera', dst: 'tool'}, 'robot', 'tcp',  geometry.poseToTransformation({
        trans: {
            x:0,
            y:0,
            z:0.05
        }
    }))
    console.log('Point in Robot-Base:\n', geometry.transformationToPose(res));

    compare(_start, res);


    // console.log('Length-Delta trans:\n', Math.sqrt(Math.pow(_diff.trans.x,2)+Math.pow(_diff.trans.y,2)+Math.pow(_diff.trans.z,2)));
});