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

#define angle float
#define dist  float
#define fromRads(A) (A)*(180/M_PI)
#define toRads(A)   (A)*(M_PI/180)

#define d2r(A)  (A)*(M_PI/180)
#define r2d(A)  (A)*(180/M_PI)

point2d_t b1 = {21.5,  60};
point2d_t b2 = {101.75,44.25};
point2d_t b3 = {61.5, -9.25};

point2d_t r  = {60.5, 30};


angle dir(dist x, dist y){
  return fromRads(atan(y/x));
}


int main(void) {

  // beacons are b1 (a) b2 (b) and b3 (c)
  // robot is r
  // All the positions are known, calc hte radar angles

  angle RA, RB, RC; // angles from radar line to horiz axis
  angle ARB, BRC, ARC; // angles between radar beams
  float D, M, acr;

  RA = fromRads(atan((r.y-b1.y) / (r.x-b1.x))); 
  RB = fromRads(atan((r.y-b2.y) / (r.x-b2.x)));
  RC = fromRads(atan((r.y-b3.y) / (r.x-b3.x)));
  
  printf("angle 1 = %f\n", RA);
  printf("angle 2 = %f\n", RB);
  printf("angle 3 = %f\n", RC);
  
  ARB = 180 + RA - RB;
  BRC = RB  - RC;
  ARC = 180 + RC - RA;

  printf("L ARB = %f\n", ARB);
  printf("L BRC = %f\n", BRC);
  printf("L ARC = %f\n", ARC);
  
  // get that dasterdly angle
  D = d2r( ABC + BRC + ACB - 180 );
  M = (AC/AB) * (sin(d2r(ARB))/sin(d2r(ARC)));
  acr = r2d(atan(sin(D)/(M + cos(D))));
  
  
  
  return(0);
}