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

#include "VascularNetwork.h"
#include "Utils.h"
#include "Bifurcation.h"
#include <iterator>
#include <math.h>

using namespace std;
 
VascularNetwork::VascularNetwork(Vessel* rootVessel) {
    this->_root = rootVessel;
    this->_vesselArray.push_back(rootVessel);
    this->_numVessels = 1;
}

VascularNetwork::~VascularNetwork() {
    printf("Destroying tree.\n");
    //listTree(_root);
    vector<Vessel *>::iterator it;
//    int i=0;
    for( it=_vesselArray.begin(); it!=_vesselArray.end(); ++it )
    {
//        Point3D p1 = (*it)->getInlet();
//        Point3D p2 = (*it)->getOutlet();
//        i++;
//        printf("Vessel [%d]: <%g,%g,%g> - <%g,%g,%g> - radius=%g.\n",(*it)->getID(), p1.x,p1.y,p1.z,p2.x,p2.y,p2.z,(*it)->getRadius());
//        printf("draw (%g,%g)--(%g,%g) withpen pencircle scaled %g;\n", p1.x,p1.z,p2.x,p2.z,(*it)->getRadius());
//        printf("Vessel [%d]: flow=%g.\n",(*it)->getID(), (*it)->getFlow());

        (*it)->DeleteVesel(false);
    }
}

int VascularNetwork::getIndexOfTheNearestVesselToPoint(Point3D terminal) {
    
    int index = 0;
    double minDistance = 10e10;
    for (int i=0; i<_numVessels; i++) {
        Point3D pA = _vesselArray.at(i)->getInlet();
        Point3D pB = _vesselArray.at(i)->getOutlet();
        
//        Point3D vectorA;
//        setPoint(&vectorA, terminal.x-pA.x, terminal.y-pA.y, terminal.z-pA.z);
//        Point3D vectorB;
//        setPoint(&vectorB, terminal.x-pB.x, terminal.y-pB.y, terminal.z-pB.z);
//
//        double crossProductABTerminalNorm = crossProductNorm(vectorA, vectorB);
//        double vesselLength = distanceP2P(pA, pB);
        
//        double distance = crossProductABTerminalNorm / vesselLength;
        
        double distance2 = dist_Point_to_Segment(terminal, pA, pB);
//        printf("Trying vessel %d (ID = %d). Distance = %g. Distance2 = %g.\n", i, _vesselArray.at(i)->getID(), distance, distance2);
        if (distance2 < minDistance) {
            minDistance = distance2;
            index = i;
        }
//        printf("i=%d\t",i);
    }
//    printf("index=%d\n", index);
    return index;
}


void VascularNetwork::scaleRadiiAndUpdatePressuresDownSubtree(Vessel* topVessel, double scalingFactor) {

//    printf("scale Radii. Factor = %1.12f.\n", scalingFactor);
    double Pout = PressureAtOutlets;
    if (topVessel->isParent()) {
        scaleRadiiAndUpdatePressuresDownSubtree(topVessel->getSon(), scalingFactor);
        scaleRadiiAndUpdatePressuresDownSubtree(topVessel->getDaughter(), scalingFactor);
        Pout = topVessel->getSon()->getPressureIn();
        topVessel->setPressureOut(Pout);
    }
    double r0 = topVessel->getRadius() * scalingFactor;
    topVessel->setRadius(r0);
    double f0 = topVessel->getFlow();
    double l0 = distanceP2P(topVessel->getInlet(), topVessel->getOutlet());
    double Pin = Pout + 8*f0*BloodViscosityInPa_s*l0/(M_PI*pow(r0, 4));
    topVessel->setPressureIn(Pin);
}

void VascularNetwork::optimizeSubtree(Vessel* topVessel) {
    
    
    if (!topVessel->isParent()) {
        topVessel->setRadius(MinimumRadiusInmm);
        topVessel->setPressureOut(PressureAtOutlets);
        return;
    }
    else {
        if (!topVessel->getSon()->isParent() && !topVessel->getDaughter()->isParent()) {
            Vessel* son = topVessel->getSon();
            Vessel* daughter = topVessel->getDaughter();
            double r1 = son->getRadius();
            double r2 = daughter->getRadius();
            double f1 = son->getFlow();
            double f2 = daughter->getFlow();
            double P1out = son->getPressureOut();
            double P2out = daughter->getPressureOut();
            double l1 = distanceP2P(son->getInlet(), son->getOutlet());
            double l2 = distanceP2P(daughter->getInlet(), daughter->getOutlet());
            
            double Pb1 = P1out + 8*f1*BloodViscosityInPa_s*l1/(M_PI*pow(r1, 4));
            double Pb2 = P2out + 8*f2*BloodViscosityInPa_s*l2/(M_PI*pow(r2, 4));
            
            double Pbout = Pb1;
            if (Pb1 > Pb2) {
                r2 = pow(8*f2*BloodViscosityInPa_s*l2/(M_PI*(Pbout-P2out)), 0.25);
            }
            else if (Pb1 < Pb2) {
                Pbout = Pb2;
                r1 = pow(8*f1*BloodViscosityInPa_s*l1/(M_PI*(Pbout-P1out)), 0.25);
            }
            son->setRadius(r1);
            daughter->setRadius(r2);
            son->setPressureIn(Pbout);
            daughter->setPressureIn(Pbout);
            
            topVessel->setPressureOut(Pbout);
            double r0 = radiusFromBifurcationLaw(topVessel, son, daughter, BifurcationLawPower);
            double f0 = topVessel->getFlow();
            double l0 = distanceP2P(topVessel->getInlet(), topVessel->getOutlet());
            double Pbin = Pbout + 8*f0*BloodViscosityInPa_s*l0/(M_PI*pow(r0, 4));
            topVessel->setPressureIn(Pbin);
            topVessel->setRadius(r0);
        }
        else {
            if (topVessel->getSon()->isParent()) {
                optimizeSubtree(topVessel->getSon());
            }
            if (topVessel->getDaughter()->isParent()) {
                optimizeSubtree(topVessel->getDaughter());
            }
            Vessel* son = topVessel->getSon();
            Vessel* daughter = topVessel->getDaughter();
            double Pb1 = son->getPressureIn();
            double Pb2 = daughter->getPressureIn();
            
            double Pbout = Pb1;
            double factor = 1.;
//            printf("in optimizeSubtree(..). Pb1 = %1.12f, Pb2 = %1.12f, Pbout = %g\n", Pb1, Pb2, Pbout);
            if (Pb1 > Pb2) {
                factor = pow((Pb2 - PressureAtOutlets)/(Pbout - PressureAtOutlets), 0.25);
//                printf("In optimizeSubtree(..); Pb1 > Pb2: calling scaleRadii with factor %g...\n", factor);
                scaleRadiiAndUpdatePressuresDownSubtree(daughter, factor);
            }
            else if (Pb1 < Pb2) {
                Pbout = Pb2;
                factor = pow((Pb1 - PressureAtOutlets)/(Pbout - PressureAtOutlets), 0.25);
//                printf("In optimizeSubtree(..); Pb1 < Pb2: calling scaleRadii with factor %g...\n", factor);
                
                scaleRadiiAndUpdatePressuresDownSubtree(son, factor);
            }
            double r0 = radiusFromBifurcationLaw(topVessel, son, daughter, BifurcationLawPower);
//            r0 *= factor;
            topVessel->setRadius(r0);
            topVessel->setPressureOut(Pbout);
            
            double f0 = topVessel->getFlow();
            double l0 = distanceP2P(topVessel->getInlet(), topVessel->getOutlet());
            double Pbin = Pbout + 8*f0*BloodViscosityInPa_s*l0/(M_PI*pow(r0, 4));
            topVessel->setPressureIn(Pbin);
        }
    }
    
}

void VascularNetwork::addNewTerminal(Point3D newTerminal) {
    
    // Find nearest vessel
    int parrentVesselIdx = getIndexOfTheNearestVesselToPoint(newTerminal);
    printf("nearest vessel id: %d\n", parrentVesselIdx);
    // Optimize bifurcation point
    Vessel* old_parent = _vesselArray.at(parrentVesselIdx);
    Bifurcation* b = new Bifurcation(old_parent, newTerminal);
    b->optimize();
    
    // Replace parrent vessel with new bifurcation
    Vessel* new_son = new Vessel(b->getSon());
    if (old_parent->isParent()) {
        new_son->setChildren(old_parent->getSon(), old_parent->getDaughter());
    }
    
    
    Vessel* new_daughter = new Vessel(b->getDaughter());
    Vessel* new_parent = new Vessel(b->getParent());
    
    new_parent->setChildren(new_son, new_daughter);
    if (old_parent->hasParent()) {
        new_parent->setParent(old_parent->getParent());
        if (old_parent->getParent()->getSon()->getID()==old_parent->getID()) {
            old_parent->getParent()->setChildren(new_parent, old_parent->getParent()->getDaughter());
        }
        else {
            old_parent->getParent()->setChildren(old_parent->getParent()->getSon(),new_parent);
        }
    }
    else {
        _root = new_parent;
        printf("New root id = %d\n", new_parent->getID());
    }
    _root->accumulateFlow();
    
    _vesselArray.erase(_vesselArray.begin() + parrentVesselIdx);
    old_parent->DeleteVesel(false);
    _vesselArray.push_back(new_parent);
    _vesselArray.push_back(new_son);
    _vesselArray.push_back(new_daughter);
    delete b;

    // Recalculate all networks radii and pressures
    optimizeSubtree(_root);

    // Ensure the given pressure at the entry of the tree
    double Pin = _root->getPressureIn();
//    double global_factor = pow((EntryPressure-PressureAtOutlets)/(Pin-PressureAtOutlets), 0.25);
    double global_factor = pow((Pin-PressureAtOutlets)/(EntryPressure-PressureAtOutlets), 0.25);
    printf("In addNewTerminal(..); calling globally scaleRadii...\n");
    scaleRadiiAndUpdatePressuresDownSubtree(_root, global_factor);

    printf("Tree optimized.\n");
    listTree(_root);
    _numVessels += 2;
}

void VascularNetwork::listTree(Vessel* topVessel) {
    
//    if (!topVessel->isParent()) {
//        printf("Vessel %d is child of %d. Flow = %g.\n", topVessel->getID(), topVessel->getParent()->getID(),topVessel->getFlow());
//    }
//    else {
//        listTree(topVessel->getSon());
//        listTree(topVessel->getDaughter());
//        if (topVessel->hasParent()) {
//            printf("Vessel %d is child of %d. Flow = %g.\n", topVessel->getID(), topVessel->getParent()->getID(), topVessel->getFlow());
//        }
//        else {
//            printf("Vessel %d is the root. Flow = %g.\n", topVessel->getID(), topVessel->getFlow());
//        }
//   
//    }
    
    vector<Vessel *>::iterator it;
    for( it=_vesselArray.begin(); it!=_vesselArray.end(); ++it )
    {
        Point3D p1 = (*it)->getInlet();
        Point3D p2 = (*it)->getOutlet();

        printf("draw (%g,%g)--(%g,%g) withpen pencircle scaled %g withcolor 0.7white;\n", p1.x,p1.z,p2.x,p2.z,(*it)->getRadius());
        
    }
    for( it=_vesselArray.begin(); it!=_vesselArray.end(); ++it )
    {
        Point3D p2 = (*it)->getOutlet();
        printf("label.rt(btex \\textcolor{blue}{\\tiny %d} etex, (%g,%g));\n", (*it)->getID(),p2.x,p2.z);
        
    }
//    vector<Vessel *>::iterator it;
//
//    for( it=_vesselArray.begin(); it!=_vesselArray.end(); ++it )
//    {        printf("Vessel %d. Radius: %g. ", (*it)->getID(), (*it)->getRadius());
//        if ((*it)->hasParent()) {
//            printf("Parent: %d.\t", (*it)->getParent()->getID());
//        }
//        else {
//            printf("Root.\t");
//        }
//        if ((*it)->isParent()) {
//            printf("Son: %d.\tDaughter: %d.\n", (*it)->getSon()->getID(), (*it)->getDaughter()->getID());
//        }
//        else {
//            printf("\n");
//        }
//    }
}

void VascularNetwork::writeTree(FILE *fp) {
    fprintf(fp, "Entry_x\tEntry_y\tEntry_z\tExit_x\tExit_y\tExit_z\tRadius\tLength\tAxis_x\tAxis_y\tAxis_z\tFlow\n");
    vector<Vessel *>::iterator it;
    for( it=_vesselArray.begin(); it!=_vesselArray.end(); ++it )
    {
        Point3D p1 = (*it)->getInlet();
        Point3D p2 = (*it)->getOutlet();
        double length = distanceP2P(p1, p2);
        double radius = (*it)->getRadius();
        double flow = (*it)->getFlow();
        fprintf(fp, "%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\t%g\n",p1.x,p1.y,p1.z,p2.x,p2.y,p2.z,radius,length,p2.x-p1.x,p2.y-p1.y,p2.z-p1.z,flow);
    }
    
}
