import {Mat4} from './linalg';
import * as se3 from '../se3';

export interface Quat {
    x: number,
    y: number,
    z: number,
    w: number,
}

export function mat2Quat(m: Mat4) : Quat {
    const q : Quat = {};

    if (m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] > 0.0) {
        const t = + m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0;
        const s = 0.5 / Math.sqrt(t);
        q.w = s * t;
        q.z = (m[1 * 4 + 0] - m[0 * 4 + 1]) * s;
        q.y = (m[0 * 4 + 2] - m[2 * 4 + 0]) * s;
        q.x = (m[2 * 4 + 1] - m[1 * 4 + 2]) * s;
    } else if (m[0 * 4 + 0] > m[1 * 4 + 1] && m[0 * 4 + 0] > m[2 * 4 + 2]) {
        const t = + m[0 * 4 + 0] - m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0;
        const s = 0.5 / Math.sqrt(t);
        q.x = s * t;
        q.y = (m[1 * 4 + 0] + m[0 * 4 + 1]) * s;
        q.z = (m[0 * 4 + 2] + m[2 * 4 + 0]) * s;
        q.w = (m[2 * 4 + 1] - m[1 * 4 + 2]) * s;
    } else if (m[1 * 4 + 1] > m[2 * 4 + 2]) {
        const t = - m[0 * 4 + 0] + m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0;
        const s = 0.5 / Math.sqrt(t);
        q.y = s * t;
        q.x = (m[1 * 4 + 0] + m[0 * 4 + 1]) * s;
        q.w = (m[0 * 4 + 2] - m[2 * 4 + 0]) * s;
        q.z = (m[2 * 4 + 1] + m[1 * 4 + 2]) * s;
    } else {
        const t = - m[0 * 4 + 0] - m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0;
        const s = 0.5 / Math.sqrt(t);
        q.z = s * t;
        q.w = (m[1 * 4 + 0] - m[0 * 4 + 1]) * s;
        q.x = (m[0 * 4 + 2] + m[2 * 4 + 0]) * s;
        q.y = (m[2 * 4 + 1] + m[1 * 4 + 2]) * s;
    }

    return q;
}

export function quat2Mat(q: Quat): Mat4 {
    const m: Mat4 = se3.identity();

    const x2 = q.x + q.x;
    const y2 = q.y + q.y;
    const z2 = q.z + q.z;
    {
        const xx2 = q.x * x2;
        const yy2 = q.y * y2;
        const zz2 = q.z * z2;
        m[0 * 4 + 0] = 1.0 - yy2 - zz2;
        m[1 * 4 + 1] = 1.0 - xx2 - zz2;
        m[2 * 4 + 2] = 1.0 - xx2 - yy2;
    }
    {
        const yz2 = q.y * z2;
        const wx2 = q.w * x2;
        m[1 * 4 + 2] = yz2 - wx2;
        m[2 * 4 + 1] = yz2 + wx2;
    }
    {
        const xy2 = q.x * y2;
        const wz2 = q.w * z2;
        m[0 * 4 + 1] = xy2 - wz2;
        m[1 * 4 + 0] = xy2 + wz2;
    }
    {
        const xz2 = q.x * z2;
        const wy2 = q.w * y2;
        m[2 * 4 + 0] = xz2 - wy2;
        m[0 * 4 + 2] = xz2 + wy2;
    }

    return m;
}

export function normalize(q: Quat) : Quat {
    const n = norm(q);
    if (n < 1e-10) {
        return {x: 0, y: 0, z: 0, w: 1};
    }
    return {
        x: q.x / n,
        y: q.y / n,
        z: q.z / n,
        w: q.w / n,
    };
}

export function diff(q0: Quat, q1: Quat) {
    return {
        x: q0.x - q1.x,
        y: q0.y - q1.y,
        z: q0.z - q1.z,
        w: q0.w - q1.w,
    };
}
export function norm(q: Quat) {
    return Math.sqrt(q.x**2 + q.y**2 + q.z**2 + q.w**2);
}

export function add(q0: Quat, q1: Quat) {
    return {
        x: q0.x + q1.x,
        y: q0.y + q1.y,
        z: q0.z + q1.z,
        w: q0.w + q1.w,
    };
}

export function scale(q: Quat, a: number): Quat {
    return {
        x: a * q.x,
        y: a * q.y,
        z: a * q.z,
        w: a * q.w,
    };
}