#include "math.h"

double distance(double x_a, double y_a, double x_b, double y_b) {
        return sqrt((x_b - x_a)*(x_b - x_a)+(y_b - y_a)*(y_b - y_a));
}

// equation de la droite passant par les points A (x_a, y_a) et B (x_b, yb)
// Condition : A != B !!!!
// y = mx + p
void equation_droite(double x_a, double y_a, double x_b, double y_b,
                double *m, double *p) {
        (*m) = (y_b - y_a)/(x_b - x_a);
        (*p) = y_a - (*m)*x_a;
}

// on suppose que I se trouve sur la droite (AB)
// I se trouve-t-il dans le segment [AB] ?
int interieur_segment(double x_a, double y_a, double x_b, double y_b, double x_i, double y_i) {
        if (x_a <= x_b) {
                return (x_i >= x_a)&&(x_i <= x_b);
        } else {
                return (x_i >= x_b)&&(x_i <= x_a);
        }
}

// coordonnées du point I intersection de la droite y=mx+p (droite AB) et de la
// droite perpendiculaire passant par le point O (x_o, y_o)
// Ce point I est le point le plus proche de O sur la droite (AB)
void coord_I(double m, double p, double x_o, double y_o, double *x_i, double *y_i) {
        double m1, p1;

        m1 = (-1.0 / m);
        p1 = y_o + m1*x_o;

        (*x_i) = (p1 - p)/(m - m1);
        (*y_i) = m * (*x_i) + p;
}


int detect_collision(double x_a, double y_a, double x_b, double y_b, double x_o, double y_o, double rayon_robot,
                double *distance_decalage) {
        double m, p;
        double x_i, y_i;

        // calcule l'equation de droite (AB)
        equation_droite(x_a, y_a, x_b, y_b, &m, &p);

        // calcule le point I appartenant à (AB) et le plus proche du point O
        coord_I(m, p, x_o, y_o, &x_i, &y_i);

        // si I n'est pas dans [AB] on prend le point du segment (A ou B) le plus proche de I 
        if (! (interieur_segment(x_a, y_a, x_b, y_b, x_i, y_i))) {
                if (fabs(x_a - x_i)< fabs(x_b - x_i)) {
                        x_i=x_a;
                        y_i=y_a;
                } else {
                        x_i=x_b;
                        y_i=y_b;
                }
        }

        // la distance dont le robot doit se décaler est de rayon robot - ||OI||
        (*distance_decalage)=rayon_robot-distance(x_o, y_o, x_i, y_i);

        return ((*distance_decalage)>0);
}