//
//  Utils.cpp
//  BasicVascularModeling
//
//  Created by Artur Klepaczko on 27.05.2014.
//  Copyright (c) 2014 Artur Klepaczko. All rights reserved.
//

#include "Utils.h"
#include <math.h>
#define dot(u,v)   ((u).x * (v).x + (u).y * (v).y + (u).z * (v).z)
#define norm(v)     sqrt(dot(v,v))     // norm = length of  vector

double distanceP2P(Point3D pA, Point3D pB) {
    return sqrt(pow(pA.x-pB.x, 2) + pow(pA.y-pB.y, 2) + pow(pA.z-pB.z, 2));
}

void setPoint(Point3D* p, double x, double y, double z) {
    p->x = x;
    p->y = y;
    p->z = z;
}

double crossProductNorm(Point3D vA, Point3D vB) {
    
    double result = 0;
    
    double lengthA = sqrt(pow(vA.x, 2) + pow(vA.y, 2) + pow(vA.z, 2));
    double lengthB = sqrt(pow(vB.x, 2) + pow(vB.y, 2) + pow(vB.z, 2));
    
    Point3D unitA;
    setPoint(&unitA, vA.x/lengthA, vA.y/lengthA, vA.z/lengthA);
    Point3D unitB;
    setPoint(&unitB, vB.x/lengthB, vB.y/lengthB, vB.z/lengthB);
    
    double unitDotProductSquared = pow(unitA.x*unitB.x + unitA.y*unitB.y + unitA.z*unitB.z,2);
    
    result = lengthA*lengthB*sqrt(1-unitDotProductSquared);
    return result;
}

double distanceP2Vector(Point3D x0, Point3D x1, Point3D x2) {
    double result = 0;
    
    Point3D v12;
    setPoint(&v12, x1.x-x2.x, x1.y-x2.y, x1.z-x2.z);
    
    Point3D v10;
    setPoint(&v10, x1.x-x0.x, x1.y-x0.y, x1.z-x0.z);
    
    
    double denominator = pow(v12.x,2) + pow(v12.y,2) + pow(v12.z,2);
    
    double numerator = (pow(v10.x,2) + pow(v10.y,2) + pow(v10.z,2)) * (pow(v12.x,2) + pow(v12.y,2) + pow(v12.z,2));
    
    double dot_product = v12.x*v10.x + v12.y*v10.y + v12.z*v10.z;
    double subractor = pow(dot_product, 2);
    
    result = sqrt((numerator - subractor)/denominator);
    return result;
}

double radiusFromBifurcationLaw(Vessel* parent, Vessel*son, Vessel* daughter, double gamma) {
    double r0 = 0;
    double f0 = parent->getFlow();
    double f1 = son->getFlow();
    double f2 = daughter->getFlow();
    double r1 = son->getRadius();
    double r2 = daughter->getRadius();
    double aux_sum = f0/f1*pow(r1, gamma) + f0/f2*pow(r2,gamma);
    r0 = pow(aux_sum, 1/gamma);
//    r0 = pow(pow(r1, gamma) + pow(r2, gamma), 1/gamma);
    return r0;
}

void printPoint(Point3D p, char* label) {
    printf("Point %s: <%g, %g, %g>\n", label, p.x, p.y, p.z);
}

double dist_Point_to_Segment(Point3D x0, Point3D x1, Point3D x2)
{
    Point3D v;
    setPoint(&v, x2.x-x1.x, x2.y-x1.y, x2.z-x1.z);
    
    Point3D w;
    setPoint(&w, x0.x-x1.x, x0.y-x1.y, x0.z-x1.z);
    
    Point3D w2;
    setPoint(&w2, x0.x-x2.x, x0.y-x2.y, x0.z-x2.z);
    
    double c1 = dot(w,v);
    
    if ( c1 <= 0 )
        return norm(w);
    
    double c2 = dot(v,v);
    if ( c2 <= c1 )
        return norm(w2);
    
    double b = c1 / c2;
    Point3D Pb;
    setPoint(&Pb, x1.x+b*v.x, x1.y+b*v.y, x1.z+b*v.z);
    
    Point3D d;
    setPoint(&d, x0.x-Pb.x, x0.y-Pb.y, x0.z-Pb.z);
    return norm(d);
}