"use strict";
var __importDefault = (this && this.__importDefault) || function (mod) {
    return (mod && mod.__esModule) ? mod : { "default": mod };
};
Object.defineProperty(exports, "__esModule", { value: true });
exports.transform = exports.convertHMtoXYZABC = exports.convertXYZABCtoHM = exports.waitForState = void 0;
const utils_1 = require("@things-factory/utils");
const math3d_1 = __importDefault(require("math3d"));
async function waitForState(client, checkFn) {
    var robotStatus = await client.getRobotStatus();
    while (!checkFn(robotStatus)) {
        await (0, utils_1.sleep)(1000);
        robotStatus = await client.getRobotStatus();
    }
}
exports.waitForState = waitForState;
function convertXYZABCtoHM(xyzabc) {
    var H = math3d_1.default.Matrix4x4;
    var [x, y, z, a, b, c] = xyzabc;
    a = (a * Math.PI) / 180;
    b = (b * Math.PI) / 180;
    c = (c * Math.PI) / 180;
    var ca = Math.cos(a);
    var sa = Math.sin(a);
    var cb = Math.cos(b);
    var sb = Math.sin(b);
    var cc = Math.cos(c);
    var sc = Math.sin(c);
    H = new math3d_1.default.Matrix4x4([
        cb * cc,
        cc * sa * sb - ca * sc,
        sa * sc + ca * cc * sb,
        x,
        cb * sc,
        ca * cc + sa * sb * sc,
        ca * sb * sc - cc * sa,
        y,
        -sb,
        cb * sa,
        ca * cb,
        z,
        0,
        0,
        0,
        1
    ]);
    return H;
}
exports.convertXYZABCtoHM = convertXYZABCtoHM;
function convertHMtoXYZABC(hm) {
    var H = math3d_1.default.Matrix4x4;
    var a = 0.0;
    var b = 0.0;
    var c = 0.0;
    H = hm;
    var x = H.m14;
    var y = H.m24;
    var z = H.m34;
    if (H.m31 > 1.0 - 1e-10) {
        b = -Math.PI / 2;
        a = 0;
        c = Math.atan2(-H.m23, H.m22);
    }
    else if (H.m31 < -1.0 + 1e-10) {
        b = Math.PI / 2;
        a = 0;
        c = Math.atan2(H.m23, H.m22);
    }
    else {
        b = Math.atan2(-H.m31, Math.sqrt(H.m11 * H.m11 + H.m21 * H.m21));
        c = Math.atan2(H.m21, H.m11);
        a = Math.atan2(H.m32, H.m33);
    }
    return [x, y, z, (a * 180) / Math.PI, (b * 180) / Math.PI, (c * 180) / Math.PI];
}
exports.convertHMtoXYZABC = convertHMtoXYZABC;
function transform(base, offset) {
    var hmTcp2Base = math3d_1.default.Matrix4x4;
    var hmToolOffset = math3d_1.default.Matrix4x4;
    var hmRepos = math3d_1.default.Matrix4x4;
    var xyzuvw = [0.0, 0.0, 0.0, 0.0, 0.0];
    //   console.log(offset)
    //   console.error(offset)
    var [bx, by, bz, bu, bv, bw] = base;
    var { x: ox, y: oy, z: oz, u: ou, v: ov, w: ow } = offset;
    hmTcp2Base = convertXYZABCtoHM(base);
    hmToolOffset = convertXYZABCtoHM(offset);
    hmRepos = hmTcp2Base.mul(hmToolOffset);
    xyzuvw = convertHMtoXYZABC(hmRepos);
    return xyzuvw;
}
exports.transform = transform;
//# sourceMappingURL=util.js.map