/*****************************************************************************
 * $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$
 ****************************************************************************/

// Tools includes
#include "Tools.h"

#include <iostream>

// -------------------- distanceToTriangle --------------------
double distanceToTrianglePlane(double point[3], double tri1[3], double tri2[3], double tri3[3]) {
    double norm[3];
    double v1_2[3];
    double v1_3[3];
    double v1_0[3];
    for (int i = 0; i < 3; i++) {
        v1_2[i] = tri2[i] - tri1[i];
        v1_3[i] = tri3[i] - tri1[i];
        v1_0[i] = point[i] - tri1[i];
    }
    crossProduct(v1_2, v1_3, norm);

    //distance to projection on triangle, it is the seeked distance if the projection is inside the triangle
    double dist = distance(point, tri1) * dotProduct(v1_0, norm) / (normOf(v1_0) * normOf(norm));
    if (dist < 0) {
        dist = -dist;
    }

    //approximation here, return min between dist calculated and distance with nodes of the triangle
    //TODO find true distance  (i.e. if the minimum diatance is between the point and one edge of the triangle)
    /*double min=dist;
    double dists[3];
    dists[0]=distance(point,tri1);
    dists[1]=distance(point,tri2);
    dists[2]=distance(point,tri3);
    for(int i=0;i<3;i++){
      if (dists[i]>min)
        min=dists[i];
    }
    return min;*/

    return dist;

}


// -------------------- timeParameter2double --------------------
double timeParameter2double(mml::TimeParameter& t) {
    switch (t.unit()) {
        case mml::TimeUnit::ms:
            return 0.001 * t.value();
            break;
        case mml::TimeUnit::s:
            return 1 * t.value();
            break;
        case mml::TimeUnit::min:
            return 60 * t.value();
            break;
        default:
            std::cerr << "Time unit error" << std::endl;
    }
    return 0;
}
