/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2025 Univ. Grenoble Alpes, CNRS, Grenoble INP - UGA, TIMC, 38000 Grenoble, France
 *
 * Visit http://camitk.imag.fr for more information
 *
 * This file is part of CamiTK.
 *
 * CamiTK is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * CamiTK is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with CamiTK.  If not, see <http://www.gnu.org/licenses/>.
 *
 * $CAMITK_LICENCE_END$
 ****************************************************************************/

#ifndef TOOLS_TOOLS_H
#define TOOLS_TOOLS_H

#include <MonitoringModel.hxx>
#include <cmath>

/// compute euclidian distance
double distance(double pos[3], double pos2[3]);

/** compute the distance of a point to the plane defined by a 3D triangle.
 * It is the distance between the given point and its projection on the triangle plane.
 *
* @param point coords of the point
* @param tri1 coords of the 1st point of the triangle
* @param tri2 coords of the 2nd point of the triangle
* @param tri3 coords of the 3rd point of the triangle
*/
double distanceToTrianglePlane(double point[3], double tri1[3], double tri2[3], double tri3[3]);

/// convert a TimeParameter (from xsd-cxx generetaed file) to double
double timeParameter2double(mml::TimeParameter& t);

/// compute dot product
double dotProduct(double vec1[3], double vec2[3]);

/// compute cross product
void crossProduct(double vec1[3], double vec2[3], double res[3]);

/// normalize vector
void normalize(double vec[3]);

/// norm of vector
double normOf(double vec[3]);

// -------------------- distance --------------------
inline double distance(double pos[3], double pos2[3]) {
    return sqrt((pos[0] - pos2[0]) * (pos[0] - pos2[0])
                + (pos[1] - pos2[1]) * (pos[1] - pos2[1])
                + (pos[2] - pos2[2]) * (pos[2] - pos2[2]));
}

// -------------------- dotProduct --------------------
inline double dotProduct(double vec1[3], double vec2[3]) {
    return vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2];
}

// -------------------- distance --------------------
inline void crossProduct(double vec1[3], double vec2[3], double res[3]) {
    res[0] = vec1[1] * vec1[2] - vec1[2] * vec1[1];
    res[1] = vec1[2] * vec1[0] - vec1[0] * vec1[2];
    res[2] = vec1[0] * vec1[1] - vec1[1] * vec1[0];
}

// -------------------- normalize --------------------
inline void normalize(double vec[3]) {
    double norm = sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
    if (norm > 0) {
        vec[0] = vec[0] / norm;
        vec[1] = vec[1] / norm;
        vec[2] = vec[2] / norm;
    }
}

// -------------------- normOf --------------------
inline double normOf(double vec[3]) {
    return sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
}

#endif // TOOLS_TOOLS_H