#include "2d.h"
#include <stdio.h>
#include <math.h>

#define sqr(a) ((a)*(a))

typedef struct linef_s { 
  float m;
  float b;
} linef_t;

typedef struct circlef_s {
  point2d_t c;
  float r;
} circlef_t;


void printPoint( char * name, point2d_t * P) {
   printf("%s: %0.4f,%0.4f\n", name, P->x, P->y);
}

void printLine( char * name, linef_t * L) {
   printf("%s: y = %0.4fx + %0.4f\n", name, L->m, L->b);
}

inline void midpoint( point2d_t * A, point2d_t * B, point2d_t * result ) {
   result->x = (A->x+B->x)/2.0;
   result->y = (A->y+B->y)/2.0;
   return;
}

inline void slope(  point2d_t * A, point2d_t * B, linef_t * result ) {
  result->m = (A->y-B->y)/(A->x-B->x);
  return;
}

inline void perpendicular( linef_t * result ) {
  result->m =  (-1.0/result->m);
  return;
}

inline void pointSlopeToOff( point2d_t * A, linef_t * result) {
  result->b = A->y - (result->m * A->x);
  return;
}

void pointsToLinef (  point2d_t * A, point2d_t * B, linef_t * result ) {
   slope           ( A, B, result );
   pointSlopeToOff ( A, result );
   return;
}

void LineLineintersection ( linef_t * A, linef_t * B, point2d_t * result) {
  result->x = (A->b-B->b)/(B->m-A->m);
  result->y = ((A->m*B->b) - (A->b*B->m))/(A->m-B->m);
  return;
}


// take 3 points and find the circle that goes thu em.
void circleThru( point2d_t * A, point2d_t * B, point2d_t * C, point2d_t * result) {
  point2d_t Mab, Mbc;
  linef_t   AB,  BC;
  
  // get middle of segments
  midpoint( A, B, &Mab ); 
  midpoint( B, C, &Mbc );
  
  // find slopes
  slope( A, B, &AB); 
  slope( B, C, &BC);
  
  // invert slopes
  perpendicular( &AB);
  perpendicular( &BC);
  
  // solve lines for points
  pointSlopeToOff( &Mab, &AB);
  pointSlopeToOff( &Mbc, &BC);
  
  // find line intersections
  lineLineintersection( &AB, &BC, result);
  
  return; 
}





void IntersectTwoCircles( circlef_t * cA, circlef_t * cB, point2d_t * resultA, point2d_t * resultB) {
// Let the centers be: (a,b), (c,d)
// Let the radii be: r, s

  point2d_t  Delta;
  float      p, k;

  Delta.x = cB->c.x - cA->c.x;
//  e = c - a                             // [difference in x coordinates]
  
  Delta.y = cB->c.y - cA->c.y;
//  f = d - b                             // [difference in y coordinates]
  
  p = sqrt(sqr(Delta.x) + sqr(Delta.y));
//  p = sqrt(e^2 + f^2)                   // [distance between centers]
  
  k = (sqr(p) + sqr(cA->r) - sqr(cB->r))/(2*p);
//  k = (p^2 + r^2 - s^2)/(2p)            // [distance from center 1 to line joining points of intersection]
  
  resultA->x = cA->c.x + Delta.x*k/p + (Delta.y/p)*sqrt(sqr(cA->r )- sqr(k));                                  
//  x = a + ek/p + (f/p)sqrt(r^2 - k^2)
  
  resultA->y = cA->c.y + Delta.y*k/p - (Delta.x/p)*sqrt(sqr(cA->r )- sqr(k));
//  y = b + fk/p - (e/p)sqrt(r^2 - k^2)

  resultB->x = cA->c.x + Delta.x*k/p - (Delta.y/p)*sqrt(sqr(cA->r) - sqr(k));
//  x = a + ek/p - (f/p)sqrt(r^2 - k^2)
  
  resultB->y = cA->c.y + Delta.y*k/p + (Delta.x/p)*sqrt(sqr(cA->r )- sqr(k));
//  y = b + fk/p + (e/p)sqrt(r^2 - k^2)

}



int main(void) {

  point2d_t answerA, answerB;
  
  circlef_t c1, c2;
  
  c1.c.x = 10;
  c1.c.y = 10;
  c1.r   = 20;
  
  c2.c.x = 40;
  c2.c.y = 10;
  c2.r   = 30;

  printPoint( "c1", &c1.c);
  printPoint( "c2", &c2.c);

  IntersectTwoCircles( &c1, &c2, &answerA, &answerB);
  
  printPoint( "A", &answerA);
  printPoint( "B", &answerB);  

/*
  point2d_t A = {2,3};
  point2d_t B = {10,2};
  point2d_t C = {20,30};
  
  point2d_t answer;
  
  circleThru( &A, &B, &C, &answer);
    
  printf("Circle middle is: %f,%f\n", answer.x, answer.y);
*/
  return 0;
}