/********************************************************
Vernon Brown's personal library of graphics functions
*********************************************************/
Uint32 color = 0x55ddff;
Uint32 Red = 0xff0000, Blue = 0x0000ff;
Uint32 Yellow =  0xffff00, Greenish = 0x00ffff;
#define sign(a) (((a)<0) ? -1 : (a)>0 ? 1 : 0)
#define PI 3.141592653589793238462643383279502884197169399375105
int minx = 0, miny = 0, maxx = 1600, maxy = 800;
void proton(double, double, double);
struct prot                            /* Proton Class             */
{
    double x;                          /* spacial coordinates      */
    double y;
    double z;                          /* z coordinate as size     */
    double vx;   // x velocity component
    double vy;   // y velocity component
    double vz;   // z velocity component
    double vt;   // side C = sqrt(A * A + B * B) movement vector
    double dx;   // movement direction x
    double dy;   // movement direction y
    double ms2;  // Mass of outside shell    
    double ms3;  // Mass of middle shell 
    double ms4;  // Mass of inner shell  
    double ms;   // Total Mass 
    double q;    // Collision Quadrant 
    double p;    // Momentum = vt * (ms2 + ms3 + ms4)
    void setmass(){
      ms2 = 6.50209;
      ms3 = 42.27703;
      ms4 = 1787.36395;
      ms  = ms2 + ms3 + ms4;
    }
    void setvt(){       // Set movement vector from vx and vy
      vt = sqrt(vx * vx + vy * vy);
      dx = vx / vt;                // d conserves polarity of vx
      dy = vy / vt;
      p = vt * ms;
    }
    void setvp(){      // Set vx and vy from vt and d
      vx = vt * dx;
      vy = vt * dy;
      vz = 0;   
      p = vt * ms;
    }
    void movevtd(){
      setvp();
      if(x + vx > maxx || x + vx < minx){
        vx *= -1.0;
        setvt();        
      }
      if(y + vy > maxy || y + vy < miny){
        vy *= -1.0;
        setvt();
      }
      x += vx;
      y += vy;
      proton(x, y, z);
    }
    void avoid(double dist){
      double amt = 10.0 / (dist * dist);
      if(q == 1){ vx -= amt; vy += amt; }
      if(q == 2){ vx += amt; vy += amt; }
      if(q == 3){ vx += amt; vy -= amt; }
      if(q == 4){ vx -= amt; vy -= amt; }
      setvt();
      setvp();
    }
};

/* dot product of two protons          */
double dotp(prot & a, prot & b)
{
  double dp = a.x * b.x;               /* Do it with inline code   */
  dp += a.y * b.y;                     /* Much Faster              */
  return dp;
}


/* size is of the array containing x y coordinates */
float dotproduct(float *a, float *b, int size)
{
    float dp = 0.0f;
    for (int i=0;i<size;i++) dp += a[i] * b[i];
    return dp;
}

int collide(prot & A, prot & B){ // Proton collision detector
    if(A.x > B.x && A.y < B.y){ A.q = 3; B.q = 1; }
    if(A.x < B.x && A.y < B.y){ A.q = 4; B.q = 2; }
    if(A.x < B.x && A.y > B.y){ A.q = 1; B.q = 3; }
    if(A.x > B.x && A.y > B.y){ A.q = 2; B.q = 4; }
    double dx = A.x - B.x;
    dx *= dx;                          
    double dy = A.y - B.y;
    dy *= dy;     
    double sr = A.z + B.z;             // Shell 4 radius
    sr *= sr;
    if(dx + dy > sr * 274 * 274){
      A.avoid(dx + dy);
      B.avoid(dx + dy);
      return dx + dy; // No collision
    }
    if(dx + dy <= sr * 2)return 1;               // s4 collision
    if(dx + dy <= sr * 42 * 42) return 2;        // s3 collision
    if(dx + dy <= sr * 274 * 274) return 3;      // s2 collision  
    return 0; //Should never happen
}

void myline(unsigned int xx, unsigned int yy, 
            unsigned int xx2, unsigned int yy2, 
            Uint32 color);
static void line32(SDL_Surface *s, 
                   int x1, int y1, 
                   int x2, int y2, 
                   Uint32 color);
SDL_Surface *screen;
int init_screen(int srt, int sdn)
  {
    screen = SDL_SetVideoMode(srt, sdn, 32, SDL_SWSURFACE);
    if ( screen == NULL ) {
        fprintf(stderr, "Couldn't set video mode: %s\n",
           SDL_GetError());
        exit(1);
    }
  }
void repel(float & x, float & y, float & x2, float & y2)
{
  float a;
  a = x;
  x = x2;
  x2 = a;
  a = y;
  y = y2;
  y2 = a;
}
void putpixel(unsigned int x, unsigned int y, unsigned int color)
{
  if(x >= 0 && x < maxx && y >= 0 && y < maxy){
     unsigned int *ptr = (unsigned int*)screen->pixels;
     int lineoffset = y * (screen->pitch / 4);
     ptr[lineoffset + x] = color;
  }
}
Uint32 getpixel(SDL_Surface *surface, int x, int y)
{
    int bpp = surface->format->BytesPerPixel;
    /* Here p is the address to the pixel we want to retrieve */
    if(x >= 0 && x < 800 && y >= 0 && y < 800){
       Uint8 *p = (Uint8 *)surface->pixels + y * surface->pitch + x * bpp;
       switch(bpp) {
          case 1:
            return *p;
          case 2:
            return *(Uint16 *)p;
          case 3:
            if(SDL_BYTEORDER == SDL_BIG_ENDIAN)
                return p[0] << 16 | p[1] << 8 | p[2];
            else
                return p[0] | p[1] << 8 | p[2] << 16;
          case 4:
            return *(Uint32 *)p;
          default:
            return 0;       /* shouldn't happen, but avoids warnings */
       }
    }
    return 0; 
}

int tog(int toggle){
  if(toggle == 1) toggle = 0;
  else toggle = 1;
  return toggle;
}
int ColorFill(int color, int maxx, int maxy)
{
    int x, y;
    for(y = 0; y < maxy; y++){
       for(x = 0; x < maxx; x++){
          putpixel(x, y, color);
       }
    }
}

void MakeCircle(float xx,
                float yy,
                float radius,
                Uint32 color)
{                // Larger circle gets more dots per degree
  double inc = 1 / (radius * 2);
  double circle = 0;
  while(circle < 2 * PI){
    circle += inc;
    putpixel(xx + cos(circle) * radius, yy + sin(circle) * radius, color);
  }
}

void proton(double x, 
            double y, 
            double size)
{
  unsigned int sz;
  int stp, clr;
  double s1 = size;
  double s2 = s1 * 42;         /* Scale according to Square The Shells Rule */
  double s3 = s1 * 274;        /* in units of Shell Four                    */
  sz = (unsigned int) s1;
  if(sz < 1) sz = 1;

  MakeCircle(x, y, sz, 0xff0000);
  sz = (unsigned int) s2;
  if(sz < 1) sz = 1;
  MakeCircle(x, y, sz - 1, 0xff0000);
  MakeCircle(x, y, sz - 2, 0xff0000);
  MakeCircle(x, y, sz - 3, 0xff0000);
  MakeCircle(x, y, sz, 0xff00ff);
  MakeCircle(x, y, sz + 1, 0x0000ff);
  MakeCircle(x, y, sz + 2, 0x0000ff);
  MakeCircle(x, y, sz + 3, 0x0000ff);
  MakeCircle(x, y, sz + 4, 0x0000ff);
  sz = (unsigned int) s3;
  if(sz < 1) sz = 1;
  MakeCircle(x, y, sz - 1, 0x0000ff);
  MakeCircle(x, y, sz - 3, 0x0000ff);
  MakeCircle(x, y, sz - 5, 0x0000ff);
  MakeCircle(x, y, sz - 7, 0x0000ff);
  MakeCircle(x, y, sz - 10, 0x0000ff);
  MakeCircle(x, y, sz, 0xff00ff);
  MakeCircle(x, y, sz + 1, 0xff0000);
  MakeCircle(x, y, sz + 3, 0xff0000);
  MakeCircle(x, y, sz + 5, 0xff0000);
  MakeCircle(x, y, sz + 7, 0xff0000);
  MakeCircle(x, y, sz + 10, 0xff0000);
}
void neutron(double x, 
             double y, 
             double size)
{
  double s1 = size * 701;
  unsigned int sz = (unsigned int) s1;
  proton(x, y, size);
  MakeCircle(x, y, sz, 0xffff00);
  MakeCircle(x, y, sz - 2, 0xff0000);
  MakeCircle(x, y, sz - 4, 0xff0000);
  MakeCircle(x, y, sz - 6, 0xff0000);
  MakeCircle(x, y, sz - 10, 0xff0000);
  MakeCircle(x, y, sz - 15, 0xff0000);
  MakeCircle(x, y, sz + 2, 0x0000ff);
  MakeCircle(x, y, sz + 4, 0x0000ff);
  MakeCircle(x, y, sz + 6, 0x0000ff);
  MakeCircle(x, y, sz + 10, 0x0000ff);
  MakeCircle(x, y, sz + 15, 0x0000ff);
}
float dist(float a1, float b1, float c1, float d1){ /* returns dots between */
  float a, b, c;                                    /* two x, y, positions  */
  if(a1 > c1) a = a1 - c1;
  else a = c1 - a1;
  if(b1 > d1) b = b1 - d1;
  else b = d1 - b1;
  c = (int) sqrt((a * a) + (b * b));
  return c;
}
int collision(int x, int y, int x2, int y2, float scale)
{   /* Return 1 on collision 0 otherwise */
   float a, b, c;
   float xy_to_s2 = scale * scale;
   float xy_to_s3 = xy_to_s2 * xy_to_s2;
   if(x <= x2) b = x - x2;
   else b = x2 - x;
   if(y <= y2) c = y - y2;
   else c = y2 - y;
   a = sqrt(b * b + c * c);
   if(a > xy_to_s3 * 2)
      return 0;
   else
      return 1;
}
char keystroke(void)
{
  int count = 0;
  char keydat = 0;
  SDL_Event event;
  /* Poll for events. SDL_PollEvent() returns 0 when there are no  */
  /* more events on the event queue, our while loop will exit when */
  /* that occurs.                                                  */
  while( SDL_PollEvent( &event ) ){
    switch( event.type ){
      case SDL_KEYDOWN:
        keydat = event.key.keysym.sym;
        //printf( "Key press detected\n" );
        count ++;
        break;
      case SDL_KEYUP:
        //printf( "Key release detected\n" );
        count ++;
        break;
      case SDL_QUIT:
        keydat = 'q';
        break;
      case SDLK_KP_ENTER:
        keydat = 0x0a;
        break;
      case SDLK_RETURN:
        keydat = 0x0a;
        break;
      case SDLK_KP0:
        keydat = '0';
        break;
      case SDLK_KP1:
        keydat = '1';
        break;
      case SDLK_KP2:
        keydat = '2';
        break;
      case SDLK_KP3:
        keydat = '3';
        break;
      default:
        break;
    }
  }
  return keydat;
}
// Draw lines in 32 bit surfaces. Note that this routine
// ignores alpha values. It writes them into the surface
// if they are included in the pixel, but does nothing
// else with them.
int line(float x1, float x2, 
         float y1, float y2, 
         Uint32 color)
{
   unsigned int xx1, xx2, yy1, yy2;
   //int max = 1600;
   if(x1 < 1) x1 = 1;
   if(x2 < 1) x2 = 1;
   if(y1 < 1) y1 = 1;
   if(y2 < 1) y2 = 1;

   if(x1 > maxx) x1 = maxx;
   if(x2 > maxx) x2 = maxx;
   if(y1 > maxy) y1 = maxy;
   if(y2 > maxy) y2 = maxy;
   xx1 = (unsigned int) x1; xx2 = (unsigned int) x2;
   yy1 = (unsigned int) y1; yy2 = (unsigned int) y2; 
 
   //myline(xx1, xx2, yy1, yy2, color);           /* See bug note */

   line32(screen, xx1, xx2, yy1, yy2, color);


   return 0;

}

/* myline has a bug that causes stray dots on the screen */

void myline(unsigned int xx, unsigned int yy, 
            unsigned int xx2, unsigned int yy2, 
            Uint32 color)
{
    unsigned int x, y, x2, y2, s1, ctmp = 0;
    int up = 0, dots = 0;
    float xdist, ydist;
    float scale, s2 = 0;
    if(xx <= xx2){            /* Start point left of end point    */
       x = xx; y = yy;
       x2 = xx2; y2 = yy2;
    }
    else{
       x = xx2; y = yy2;     /* Start point to right of end point */
       x2 = xx; y2 = yy;     /* Set x to left most x point        */
    }
    xdist = x2 - x;           
    if(y < y2){
       ydist = y2 - y;
       up = 1;              /* Y plots in down direction   */
    }
    else{
       ydist = y - y2;
       up = -1;             /* Y plots in up direction     */
    }
    if(xdist > ydist){       
       dots = (int) xdist;                /* left to right */
       if(xdist > 0) scale = ydist / xdist;
       else scale = 0;
       s2 = 0;
       while(dots -- > 0){
          putpixel(x++, y + s1 * up, color);
          s2 += scale;
          s1 = (unsigned int) s2; 
       }
    }
    else{                  /* Increment plot in Y direction  */
       dots = (int) ydist; /* primary is up or down */
       if(ydist > 0) scale = xdist / ydist;
       else scale = 0;
       s2 = 0;
       while(dots-- > 0){
           putpixel(x + s1, y += up, color);
           s2 += scale;
           s1 = (unsigned int) s2;
       }
    }  
}


/* line32 crashes on exit but makes lines correctly     */
// Probably needs test to keep from plotting off screen */
static void line32(SDL_Surface *s, 
                   int x1, int y1, 
                   int x2, int y2, 
                   Uint32 color)
{
  int d;
  int x;
  int y;
  int ax;
  int ay;
  int sx;
  int sy;
  int dx;
  int dy;

  Uint8 *lineAddr;
  Sint32 yOffset;

  dx = x2 - x1;  
  ax = abs(dx) << 1;  
  sx = sign(dx);

  dy = y2 - y1;  
  ay = abs(dy) << 1;  
  sy = sign(dy);
  yOffset = sy * s->pitch;

  x = x1;
  y = y1;

  lineAddr = ((Uint8 *)(s->pixels)) + (y * s->pitch);
  if (ax>ay)
  {                      /* x dominant */
    d = ay - (ax >> 1);
    for (;;)
    {
      *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;

      if (x == x2)
      {
        return;
      }
      if (d>=0)
      {
        y += sy;
        lineAddr += yOffset;
        d -= ax;
      }
      x += sx;
      d += ay;
    }
  }
  else
  {                      /* y dominant */
    d = ax - (ay >> 1);
    for (;;)
    {
      *((Uint32 *)(lineAddr + (x << 2))) = (Uint32)color;

      if (y == y2)
      {
        return;
      }
      if (d>=0) 
      {
        x += sx;
        d -= ay;
      }
      y += sy;
      lineAddr += yOffset;
      d += ax;
    }
  }
}
void movit(float * p1)
{
  float scale;
  float dir, rad, dar, speed, x, y;
  float angle1, angle2;
  dir = p1[0];
  speed = p1[1];
  x = p1[2];
  y = p1[3];                     /* Reflection = 2 * wall   */
  scale = p1[8];                 /* angle - path angle      */
  while(dir > 360) dir -= 360;   /* Keep dir within 0 - 360 */
  while(dir < 0  ) dir += 360;
  rad = dir * (PI / 180);
  dar = (180 - dir) * (PI / 180);
  x += sin(rad) * speed;
  y += cos(dar) * speed;
  if(dir > 0 && dir <= 90){      /* Up Right  */
    if(y < 1) dir = 180 - dir;
    if(x > 799) dir = 360 - dir;
  }
  if(dir > 90 && dir <= 180){   /* Down Right */
    if(x > 799) dir = 360 - dir;
    if(y > 599) dir = 180 - dir;
  }
  if(dir > 180 && dir <= 270){  /* Down Left  */
    if(x < 1)   dir = 360 - dir;
    if(y > 599) dir = 540 - dir;
  }
  if(dir > 270 && dir <= 360){  /* Up Left    */
    if(x < 1) dir = 720 - dir;
    if(y < 1) dir = 540 - dir;
  }
  p1[0] = dir;
  p1[2] = x;
  p1[3] = y;
  proton(x, y, scale);  
}
void emf(float * xy, Uint32 * color)
{
  Uint32 pix;
  int x, y;
  float dx;
  x  = (int) xy[0];
  y  = (int) xy[1];
  dx = xy[3];
  color[1] = getpixel(screen, x-1, y);
  color[2] = getpixel(screen, x-1, y-1);
  color[3] = getpixel(screen, x, y-1);
  color[4] = getpixel(screen, x + 1, y - 1);
  color[5] = getpixel(screen, x + 1, y);
  color[6] = getpixel(screen, x + 1, y + 1);
  color[7] = getpixel(screen, x, y + 1);
  color[8] = getpixel(screen, x - 1, y + 1);
  color[0] = getpixel(screen, x, y);
}
const unsigned char sinetable[256] = {
        128,131,134,137,140,143,146,149,152,156,159,162,165,168,171,174,
        176,179,182,185,188,191,193,196,199,201,204,206,209,211,213,216,
        218,220,222,224,226,228,230,232,234,236,237,239,240,242,243,245,
        246,247,248,249,250,251,252,252,253,254,254,255,255,255,255,255,
        255,255,255,255,255,255,254,254,253,252,252,251,250,249,248,247,
        246,245,243,242,240,239,237,236,234,232,230,228,226,224,222,220,
        218,216,213,211,209,206,204,201,199,196,193,191,188,185,182,179,
        176,174,171,168,165,162,159,156,152,149,146,143,140,137,134,131,
        128,124,121,118,115,112,109,106,103,99, 96, 93, 90, 87, 84, 81, 
        79, 76, 73, 70, 67, 64, 62, 59, 56, 54, 51, 49, 46, 44, 42, 39, 
        37, 35, 33, 31, 29, 27, 25, 23, 21, 19, 18, 16, 15, 13, 12, 10, 
        9,  8,  7,  6,  5,  4,  3,  3,  2,  1,  1,  0,  0,  0,  0,  0,  
        0,  0,  0,  0,  0,  0,  1,  1,  2,  3,  3,  4,  5,  6,  7,  8,  
        9,  10, 12, 13, 15, 16, 18, 19, 21, 23, 25, 27, 29, 31, 33, 35, 
        37, 39, 42, 44, 46, 49, 51, 54, 56, 59, 62, 64, 67, 70, 73, 76, 
        79, 81, 84, 87, 90, 93, 96, 99, 103,106,109,112,115,118,121,124
};

void photon(unsigned int x, unsigned int y, int xyz, int half)
{
    int i;
    if(x < 0) x = 0;
    if(y < 0) y = 0;
    if ( SDL_MUSTLOCK(screen) ) {
        if ( SDL_LockSurface(screen) < 0 ) {
            fprintf(stderr, "Can't lock screen: %s\n", SDL_GetError());
            return;
        }
    }
    if(half == 0){ 
       line(x - xyz, y, x, y, Yellow);
       line(x, y, x + xyz, y, Greenish);
       line(x, y - xyz, x, y, Blue);
       line(x, y, x, y + xyz, Red);
    } 
    if(half == 1){
       line(x - xyz, y, x, y, Greenish);
       line(x, y, x + xyz, y, Yellow);
       line(x, y - xyz, x, y, Red);
       line(x, y, x, y + xyz, Blue);
    }
    if(half == 2){
       line(x - xyz, y, x, y, color);
       line(x, y, x + xyz, y, color);
       line(x, y - xyz, x, y, color);
       line(x, y, x, y + xyz, color);
    }
    if ( SDL_MUSTLOCK(screen) ) {
        SDL_UnlockSurface(screen);
    }    
}


    // First, find the normalized vector n from the center of
    // circle1 to the center of circle2

/*
    Vector n = circle1.center - circle2.center;
    n.normalize();

    // Find the length of the component of each of the movement
    // vectors along n.
    // a1 = v1 . n
    // a2 = v2 . n
    //float a1 = v1.dot(n);
    //float a2 = v2.dot(n);

    // Using the optimized version,
    // optimizedP =  2(a1 - a2)
    //              -----------
    //                m1 + m2
    float optimizedP = (2.0 * (a1 - a2)) / (circle1.mass + circle2.mass);

    // Calculate v1', the new movement vector of circle1
    // v1' = v1 - optimizedP * m2 * n
    //Vector v1' = v1 - optimizedP * circle2.mass * n;

    // Calculate v1', the new movement vector of circle1
    // v2' = v2 + optimizedP * m1 * n
    //Vector v2' = v2 + optimizedP * circle1.mass * n;

    //circle1.setMovementVector(v1');
    //circle2.setMovementVector(v2');

*/