#include <stdio.h>
#include <stdlib.h>
#include "ikLib.h"
#include "2d.h"



#define WIDTH  640
#define HEIGHT 480

#define CX (WIDTH/2)
#define CY (HEIGHT/2)

#define CLineRGBA(A,B,C,D,E,F,G,H,I)  lineRGBA((A),(B+CX),(-C+CY),(D+CX),(-E+CY),(F),(G),(H),(I))
#define PLineRGBA(A,B,C,D,E,F,G)  CLineRGBA((A),B.x,B.y,C.x,C.y,(D),(E),(F),(G))
#define LLineRGBA(A,B,C,D,E,F)  CLineRGBA((A),B.p1.x,B.p1.y,B.p2.x,B.p2.y,(C),(D),(E),(F))

#define deg2rad(A)  ((A)*(M_PI/180))



int main(int argc, char *argv[]) {


#define len1 10
#define len2 10
  line_t seg1, seg2;
  dist temp;
  double a,b;

  long px = 10;
  long py =  7;
  
  temp = dist3d(0, 0, 0, px, py, 0);
  b = 180-elbow(temp, len2, len1);
  a = dir(px,py) - b/2 + 270;
  
    
  initLine(&seg1);
  initLine(&seg2);  
  scaleLine(&seg1, pt(0, len2));
  scaleLine(&seg2, pt(0, len1));
  
  
  rotLine(&seg2, deg2rad(b));
  transLine(&seg2, pt(0,len2));
    
  rotLine(&seg1, deg2rad(a));
  rotLine(&seg2, deg2rad(a));  
  
 
  printf("elbow at: %f, %f\n", seg1.p1.x, seg1.p1.y);
  printf("elbow at: %f, %f\n", seg1.p2.x, seg1.p2.y);
  printf("gripper at: %f, %f\n", seg2.p1.x, seg2.p1.y);
  printf("gripper at: %f, %f\n", seg2.p2.x, seg2.p2.y);
  printf("The base angle is: %f \n",  a);
  printf("The elbow angle is: %f \n", b);
  

  return 0;
}