#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <SDL/SDL.h>

#define PI 3.141592

/* Multiply a vector with a matrix */

void mul_vector(float vector[3][1], float matrix[3][3], float dest[3][1])
{
    float num = 0;
    int i, j;

    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            num += vector[j][0] * matrix[j][i];
        }
        dest[i][0] = num;
        num = 0;
    }
    return;
}

Uint32 getp(SDL_Surface *surface, int x, int y){
 int bpp = surface->format->BytesPerPixel;
 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;
 }

}

void setp(SDL_Surface *surface, int x, int y, Uint32 pixel){
 int bpp = surface->format->BytesPerPixel;
 Uint8 *p = (Uint8 *)surface->pixels + y * surface->pitch + x * bpp;
 switch(bpp){
 case 1:
   *p = pixel;
   break;
 case 2:
   *(Uint16 *)p = pixel;
   break;
 case 3:
   if(SDL_BYTEORDER == SDL_BIG_ENDIAN){
     p[0] = (pixel >> 16) & 0xff;
     p[1] = (pixel >> 8) & 0xff;
     p[2] = pixel & 0xff;
   }else{
     p[0] = pixel & 0xff;
     p[1] = (pixel >> 8) & 0xff;
     p[2] = (pixel >> 16) & 0xff;
   }
   break;
 case 4:
   *(Uint32 *)p = pixel;
   break;
 }
}

SDL_Surface *screen;

void init(){
  SDL_Init(SDL_INIT_VIDEO);
  atexit(SDL_Quit);
  screen = SDL_SetVideoMode(640,480,24,SDL_HWSURFACE|SDL_DOUBLEBUF|SDL_HWPALETTE/*|SDL_FULLSCREEN*/);
  SDL_ShowCursor(SDL_DISABLE);
}

void plot(SDL_Surface *surface, int x, int y, Uint32 pixel){
  Uint8 *p = (Uint8 *)surface->pixels + y * surface->pitch + x;
  *p = pixel;
}

void line(SDL_Surface *surface, float x1, float y1, float x2, float y2, Uint32 c){
  float d, dx, dy, i;
  int xcalc, ycalc;

  dx = (x2 - x1);
  dy = (y2 - y1);
  d = sqrt(dx * dx + dy * dy);


  for(i = 0; i <= d; i+= 0.5) {
    xcalc = (int)(x1 + dx * (i / d));
    ycalc = (int)(y1 + dy * (i / d));
    if(xcalc > 0 && xcalc < 640 && ycalc > 0 && ycalc < 480)
      plot(surface, xcalc, ycalc, c);
  }
}

void transform(SDL_Surface *src, SDL_Surface *dst, float tmatrix[3][3]) {
   Uint32 color;

   color = 0;
   float pxl[3][1];
   float npxl[3][1];
   int x, y;

   for(y = 0; y < src->h; y++) {
      for(x = 0; x < src->w; x++) {
         color = getp(src, x, y);
         pxl[0][0] = (x-src->w/2);
         pxl[1][0] = (y-src->h/2);
         pxl[2][0] = 0; 
	 /*printf("pxl[0][0]: %f\npxl[1][0]: %f\n",pxl[0][0], pxl[1][0]);*/
         mul_vector(pxl, tmatrix, npxl);
	 
         if(color != 0)
	   if(npxl[0][0]+src->w/2 >= 0 && npxl[0][0]+src->w/2 < dst->w && npxl[1][0]+src->h/2 >= 0 && npxl[1][0]+src->h/2 < dst->h)
	     setp(dst, npxl[0][0]+src->w/2, npxl[1][0]+src->h/2, color);
      }
   }
}
      



int main(int argc, char *argv[]) {
  init();
  SDL_Surface *sprite = NULL;
  SDL_Surface *buffer = NULL;
  SDL_Event event;
  SDL_Rect rect;
  short busy;
  int i;
  int dx, dy;
  int xoff = 320;
  int yoff = 240;
  int half_width, half_height;
  float v = 1;
  float s;
   rect.x = 20;
   rect.y = 20;
   dx = dy = 3;

  sprite = SDL_LoadBMP("test2.bmp");
  if(!sprite){
    fprintf(stderr, "Could not load sprite: %s\n", SDL_GetError());
    exit(1);
  }

  buffer = SDL_CreateRGBSurface(SDL_HWSURFACE,
				sprite->w,
				sprite->h,
				sprite->format->BitsPerPixel,
				0, 0, 0, 0);

  printf("%dx%dx%d\n", 
	 sprite->w,
	 sprite->h,
	 sprite->format->BitsPerPixel);


  half_width = sprite->w / 2;
  half_height= sprite->h / 2;

  busy = 1;
  while(busy){

    while(SDL_PollEvent(&event))
      if(event.type == SDL_KEYDOWN)
        if(event.key.keysym.sym == SDLK_ESCAPE)
          busy = 0;

  SDL_FillRect(screen, NULL, SDL_MapRGB(screen->format, 0, 0, 0));

  float rmatrix[3][3] = {
    {cos(v),  sin(v), 0},
    {-sin(v), cos(v), 0},
    {0,       0,      0}
  };
  float smatrix[3][3] = {
    {v, 0, 0},
    {0, v, 0},
    {0, 0, 1},
  };
  
   rect.w = sprite->w;
   rect.h = sprite->h;
   SDL_LockSurface(sprite);
   SDL_LockSurface(buffer);
   transform(sprite, buffer, rmatrix);
   SDL_UnlockSurface(sprite);
   SDL_UnlockSurface(buffer);
   SDL_BlitSurface(buffer, NULL, screen, &rect);
   rect.x = rect.x + dx;
   rect.y = rect.y + dy;

   if(rect.x > (640-rect.w) || rect.x < 0) {
     dx = -dx;
   }
   if(rect.y > (480-rect.h) || rect.y < 0) {
     dy = -dy;
   }

   SDL_Flip(screen);
   v = v + 0.04; 

  }
  return 0;
}