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

#include <SDL/SDL.h>
#include <SDL/SDL_gfxPrimitives.h>

#define WIDTH  800
#define HEIGHT 600

#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))



void CrossHair(SDL_Surface *screen, int px, int py) ;


void Slock(SDL_Surface *screen) {
  if ( SDL_MUSTLOCK(screen) ) {
    if ( SDL_LockSurface(screen) < 0 ) {
      return;
    }
  }
}

void Sulock(SDL_Surface *screen) {
  if ( SDL_MUSTLOCK(screen) ) {
    SDL_UnlockSurface(screen);
  }
}


void CrossHair(SDL_Surface *screen, int px, int py) {
   CLineRGBA(screen, px-8, py, px+8, py, 128, 128, 128, 128);
   CLineRGBA(screen, px, py-8, px, py+8, 128, 128, 128, 128);
}

void DrawScene(SDL_Surface *screen, int px, int py) {

#define len0 28
#define len1 175
#define len2 275

/*
#define len0 0
#define len1 10
#define len2 10

*/

  dist C;
  line_t seg0, seg1, seg2;
  double a,b,c,t;
  double t1, t2;

//  px = 10; py = 7;
  
  C = dist3d(0, 0, 0, px-len0, py, 0);
  c = elbow(C, len2, len1);
  b = elbow(len2, C, len1);
  t = dir(px-len0,py);

  printf("%f\n", c);

  t1 = t + b;
  t2 = 180 + c;
  
  
  Slock(screen);
  
  // set up leg
  
  initLine(&seg0);
  initLine(&seg1);
  initLine(&seg2);
  
  scaleLine(&seg0, pt(len0, 0));
  scaleLine(&seg1, pt(len1, 0));
  scaleLine(&seg2, pt(len2, 0));
  
  rotLine  (&seg2, deg2rad(t2));
  transLine(&seg2, pt(len1, 0));
    
  rotLine  (&seg1, deg2rad(t1));
  rotLine  (&seg2, deg2rad(t1));  
  
  transLine(&seg1, pt(len0, 0));
  transLine(&seg2, pt(len0, 0));
  
  LLineRGBA(screen, seg0, 0, 255, 0,  255);
  LLineRGBA(screen, seg1, 255, 0, 0,  255);
  LLineRGBA(screen, seg2, 0, 0, 255,  255);    
  
  CrossHair(screen, 0, 0);
  
  Sulock(screen);
  SDL_Flip(screen);
}




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

  int done;
  int pass;
  SDL_Rect foo ={0, 0, WIDTH, HEIGHT};
  int x, y;

  if ( SDL_Init(SDL_INIT_AUDIO|SDL_INIT_VIDEO) < 0 ) {
    printf("Unable to init SDL: %s\n", SDL_GetError());
    exit(1);
  }
  atexit(SDL_Quit);

  SDL_Surface *screen;
  screen=SDL_SetVideoMode(WIDTH, HEIGHT, 32,SDL_HWSURFACE|SDL_DOUBLEBUF);
  if ( screen == NULL ) {
    printf("Unable to set %dx%d video: %s\n",WIDTH, HEIGHT, SDL_GetError());
    exit(1);
  }
  
  pass = 0;
  done = 0;
  while(done == 0) {
    SDL_Event event;

    while ( SDL_PollEvent(&event) ) {
      if ( event.type == SDL_QUIT )  {  done = 1;  }

      if ( event.type == SDL_KEYDOWN ) {
        if ( event.key.keysym.sym == SDLK_ESCAPE ) { done = 1; }        
       // if ( event.key.keysym.sym == 'e' )         { pass++; }
       // if ( event.key.keysym.sym == 'x' )         { pass--; }
        
      }
      
      SDL_GetMouseState(&x, &y);
      x = x-CX;
      y = -(y-CY);
            
    }
   SDL_FillRect(screen, &foo , 0);
   DrawScene(screen, x, y);
   usleep(10);
  }

  return 0;
}