/*
 *  basicas.h
 *  GC
 *
 *  Created by Chema on 10/21/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "vec.h"

// lexicographical order

template<typename T,unsigned N>
bool lessLexicoOrder(const Vec<T,N>& lhs, const Vec<T,N>& rhs) {
    bool ret=false;
    bool done=false;
    // test coords in order
    for (unsigned i=0;i<N && !done;++i) {
        if(lhs[i]<rhs[i]) {ret=true; done=true;}
        if(lhs[i]>rhs[i]) {done=true;}
    }
    return ret;
}

template<typename T,unsigned N>
bool lessLexicoOrderYX(const Vec<T,N>& lhs, const Vec<T,N>& rhs) {
    bool ret=false;
    bool done=false;
    // test coords in order
    int order[]={1,0};
    for (unsigned i=0;i<2 && !done;++i) {
        int ii = order[i];
        if(lhs[ii]<rhs[ii]) {ret=true; done=true;}
        if(lhs[ii]>rhs[ii]) {done=true;}
    }
    return ret;
}

template<typename T,unsigned N>
bool lessLexicoOrderYXZ(const Vec<T,N>& lhs, const Vec<T,N>& rhs) {
    bool ret=false;
    bool done=false;
    // test coords in order
    int order[]={1,0,2};
    for (unsigned i=0;i<3 && !done;++i) {
        int ii = order[i];
        if(lhs[ii]<rhs[ii]) {ret=true; done=true;}
        if(lhs[ii]>rhs[ii]) {done=true;}
    }
    return ret;
}

template<typename T,unsigned N>
bool ptr_lessLexicoOrderYXZ(const Vec<T,N>* lhs, const Vec<T,N>* rhs) {
    bool ret=false;
    bool done=false;
    // test coords in order
    int order[]={1,0,2};
    for (unsigned i=0;i<3 && !done;++i) {
        int ii = order[i];
        if((*lhs)[ii]<(*rhs)[ii]) {ret=true; done=true;}
        if((*lhs)[ii]>(*rhs)[ii]) {done=true;}
    }
    return ret;
}
//
//template<typename T>
//bool lessLexicoOrder(const Vec<T,2>& lhs, const Vec<T,2>& rhs) {
//    bool ret=false;
//    // test the x-coord first
//    if (lhs[0] < rhs[0]) { ret=true; }
//    else if (lhs[0] == rhs[0]) { 
//        if (lhs[1] < rhs[1]) { ret=true; } 
//    } 
//    return ret;    
//}


//Distancia entre dos puntos (longitud de un segmento)
//float dist (const Real2& a, const Real2& b);
template<typename T>
T dist (const Vec<T,2>& a, const Vec<T,2>& b) {
    return (b-a).norm();
}
//Area (signada) del paralelogramo (método de Ángel)
//float pgArea(const Real2& a, const Real2& b, const Real2& c);
template<typename T>
T pgArea(const Vec<T,2>& a, const Vec<T,2>& b, const Vec<T,2>& c) {
    return ( (b.x() - a.x())*(c.y() - a.y()) - (b.y() - a.y())*(c.x() - a.x()) );
}

//Area (signada) del triángulo
//float triArea(const Real2& a, const Real2& b, const Real2& c);
template<typename T>
T triArea(const Vec<T,2>& a, const Vec<T,2>& b, const Vec<T,2>& c) {
    return 0.5f*pgArea(a,b,c);
}

//Signo del área signada de un triángulo
//int signoArea(const Real2& a, const Real2& b, const Real2& c);
template<typename T>
int signoArea(const Vec<T,2>& a, const Vec<T,2>& b, const Vec<T,2>& c) {
    T area = pgArea(a,b,c);
    return area > 0? 1: 
           area < 0? -1:
           0;
}

//Test de tres puntos alineados
//bool alineados (const Real2& a, const Real2& b, const Real2& c);
template<typename T>
bool alineados (const Vec<T,2>& a, const Vec<T,2>& b, const Vec<T,2>& c) {
    return signoArea(a,b,c) == 0;
}

//Distancia de punto a recta (p punto, a y b puntos de linea)
float distPtRecta (const Real2& p, const Real2& a, const Real2& b);
//Distancia de punto a segmento
float distPtSegmento (const Real2& p, const Real2& a, const Real2& b);
//Distancia angular entre dos vectores
float distAngular (const Real2& a, const Real2& b);
//Distancia de punto a circunferencia
float distPtCircunf (const Real2& p, const Real2& cOri, real cRadio);
//Test de inclusión de punto en triángulo (p punto, a b c triángulo)
bool testPtEnTri (const Real2& p, const Real2& a, const Real2& b, const Real2& c) ;

//Test de inclusión de punto en segmento
//bool testPtEnSeg(const Real2& p, const Real2& a, const Real2& b) ;
template<typename T>
bool testPtEnSeg(const Vec<T,2>& p, const Vec<T,2>& a, const Vec<T,2>& b) {
    bool ret = false;
    if (alineados(a,b,p) ) {
        Vec<T,2> b2a = a-b;
        if (fabs(b2a.x()) > fabs(b2a.y()) ) {
            ret = min(a.x(),b.x())<=p.x() && p.x()<=max(a.x(),b.x());
        }
        else {
            ret = min(a.y(),b.y())<=p.y() && p.y()<=max(a.y(),b.y());
        }
    }
    return ret;
}

//Test de intersección de dos segmentos
//bool testIntersecxSegs(const Real2& a, const Real2& b, const Real2& s, const Real2& t);
template<typename T>
bool testIntersecxSegs(const Vec<T,2>& a, const Vec<T,2>& b, const Vec<T,2>& c, const Vec<T,2>& d) {
    bool ret = true;
    if (alineados(a,b,c) && alineados(a,b,d) ) 
    {
        if (testPtEnSeg(c,a,b) || testPtEnSeg(d,a,b)) {
            ret = true;
        }
    } // ret = true
    else if (signoArea(a,b,c) == signoArea(a,b,d)) {
        ret = false; // s,t está completamente en uno de los semiplano de a,b
    }
    else if (signoArea(a,c,d) == signoArea(b,c,d)) {
        ret = false; // s,t no corta a,b ni pasa por a ó b 
    }
    return ret;
}

//Intersección de dos rectas 
//Entrada:  a,b=recta1 
//          c,d=recta2
//Salida:   true si hay intersección, false caso contrario
//          solapa = true si se solapan, 
//          p = punto intersección (sólo válido si valor retorno=true y solapan=false)
//bool intersecxRectas(const Real2& a, const Real2& b, const Real2& c, const Real2& d, bool& solapan, Real2& p, float& ua, float & ub) ;
template<typename T>
bool intersecxRectas(const Vec<T,2>& a, const Vec<T,2>& b, const Vec<T,2>& c, const Vec<T,2>& d, bool& solapan, Vec<T,2>& p, T& ua, T& ub) {
    bool ret = false;
    
    T numa = (d.x()-c.x())*(a.y()-c.y()) - (d.y()-c.y())*(a.x()-c.x());
    T numb = (b.x()-a.x())*(a.y()-c.y()) - (b.y()-a.y())*(a.x()-c.x());
    T den = (d.y()-c.y())*(b.x()-a.x()) - (d.x()-c.x())*(b.y()-a.y());

    if (fabs (den) > EPSILON6) {
        ua = numa/den;
        ub = numb/den;
        p = a+ ua*(b-a);

        ret = true;
    }
    else if (fabs(numa) < EPSILON6 && fabs(numb) < EPSILON6) {
        solapan = true;
        ret = true;
    }
    return ret;
}

//Intersección de recta y segmento
//Entrada:  a,b=recta1 
//          s1,s2=segmento
//Salida:   true si hay intersección, false caso contrario
//          solapa = true si se solapan, 
//          p = punto intersección (sólo válido si valor retorno=true y solapan=false)
bool intersecxRectaSeg(const Real2& a, const Real2& b, const Real2& s1, const Real2& s2, bool& solapan, Real2& p);

// Circuncentro del triangulo (centro del circulo que pasa por los tres vertices)
bool triCcentro(const Real2& a, const Real2& b, const Real2& c, Real2& cc);

// TODO: unit testing
