#include <display_syscalls.h>

#include "FonctionUtile.h"


void* GetVRAMAdress()
{
	asm("mov.l syscall_adress, r2\n"
	    "mov.l getVRAM, r0\n"
	    "jmp @r2\n"
	    "nop\n"
	    "syscall_adress: .long 0x80020070\n"
	    "getVRAM: .long 0x01E6");
}

int sqrt_int (int i)
{
    int intervalleMin = 0, intervalleMax = 21;
    int tab_sqrt[]={0,1,4,9,16,25,36,49,64,81,100,121,144,169,196,225,256,289,324,361,400};
    while (tab_sqrt[(intervalleMax+intervalleMin)/2] != i  && intervalleMax != intervalleMin && intervalleMax != intervalleMin+1)
    {
        if (tab_sqrt[(intervalleMax+intervalleMin)/2]>i)
            intervalleMax = (intervalleMax+intervalleMin)/2;
        else
            intervalleMin = (intervalleMax+intervalleMin)/2;
    }
    return (intervalleMax+intervalleMin)/2;
}

int Rand32(int lower, int upper) {
    static int a = 123456789;
    static int b = 362436069;
    static int c = 521288629;
    static int d = 88675123;
    int t;
    t = a ^ (a << 11);
    a = b;
    b = c;
    c = d;
    return (d = d ^ (d >> 19) ^ (t ^ (t >> 8 )))%upper + lower;
}

int keydown(int basic_keycode)
{
    /* test*/
    const unsigned short* keyboard_register = (unsigned short*)0xA44B0000;
    int row, col, word, bit;
    row = basic_keycode%10;
    col = basic_keycode/10-1;
    word = row>>1;
    bit = col + 8*(row&1);
    return (0 != (keyboard_register[word] & 1<<bit));
}

/////////////////////////////////////////////////////////////////////



//draws a point of color color at (x0, y0)
void plot(int x, int y, color_t color) {
   color_t * base = y * LCD_WIDTH_PX + x +(color_t*) 0xA8000000;
   *base = color;
   return;
}


void CopySpriteNbitMasked(const unsigned char* data, int x, int y, int width, int height, const color_t* palette, color_t maskColor, unsigned int bitwidth)
{
   color_t* VRAM = (color_t*)GetVRAMAdress();
   VRAM += (LCD_WIDTH_PX*y + x);
   int offset = 0;
   unsigned char buf=0;
   int i,j;
   for(j=y; j<y+height; j++)
   {
      int availbits = 0;
      for(i=x; i<x+width;  i++)
      {
         if (!availbits)
         {
            buf = data[offset++];
            availbits = 8;
         }
         color_t thiss = ((color_t)buf>>(8-bitwidth));
         color_t color = palette[thiss];
         if(color != maskColor && i<LCD_WIDTH_PX && i>=0)
         {
            *VRAM = color;
         }
         VRAM++;
         buf<<=bitwidth;
         availbits-=bitwidth;
      }
      VRAM += (LCD_WIDTH_PX-width);
   }
}

void CopySpriteNbitMaskedLayer(const unsigned char* data, int x, int y, int width, int height, const color_t* palette, color_t maskColor, unsigned int bitwidth,bool layer, color_t layerColor,int alpha)
{
   color_t* VRAM = (color_t*)GetVRAMAdress();
   VRAM += (LCD_WIDTH_PX*y + x);
   int offset = 0;
   unsigned char buf=0;
   int i,j;
   for(j=y; j<y+height; j++)
   {
      int availbits = 0;
      for(i=x; i<x+width;  i++)
      {
         if (!availbits)
         {
            buf = data[offset++];
            availbits = 8;
         }
         color_t thiss = ((color_t)buf>>(8-bitwidth));
         color_t color = palette[thiss];
         if(color != maskColor && i<LCD_WIDTH_PX && i>=0)
         {
            *VRAM = color;
            if(layer) *VRAM = ((((layerColor & 0xF81F) * alpha + (*VRAM & 0xF81F) * (32-alpha)) >> 5) & 0xF81F) |
            ((((layerColor  & 0x07E0) * alpha + (*VRAM & 0x07E0) * (32-alpha)) >> 5) & 0x07E0);
         }
         VRAM++;
         buf<<=bitwidth;
         availbits-=bitwidth;
      }
      VRAM += (LCD_WIDTH_PX-width);
   }
}

void CopySpriteNbitMaskedAlpha(const unsigned char* data, int x, int y, int width, int height, const color_t* palette, color_t maskColor, unsigned int bitwidth,int alpha)
{
   color_t* VRAM = (color_t*) GetVRAMAdress();
   VRAM += (LCD_WIDTH_PX*y + x);
   alpha %= 32;
   int i,j;
   int offset = 0;
   unsigned char buf=0;
   for(j=y; j<y+height; j++)
   {
      int availbits = 0;
      for(i=x; i<x+width;  i++)
      {
         if (!availbits)
         {
            buf = data[offset++];
            availbits = 8;
         }
         color_t thiss = ((color_t)buf>>(8-bitwidth));
         color_t color = palette[thiss];
         if(color != maskColor&& i >=0 && i<384)
         {
            *VRAM = ((((color & 0xF81F) * alpha + (*VRAM & 0xF81F) * (32-alpha)) >> 5) & 0xF81F) |
            ((((color & 0x07E0) * alpha + (*VRAM & 0x07E0) * (32-alpha)) >> 5) & 0x07E0);
         }
         VRAM++;
         buf<<=bitwidth;
         availbits-=bitwidth;
      }
      VRAM += (LCD_WIDTH_PX-width);
   }
}

/*void SetPixelAlpha(int x, int y, color_t color, char alpha) {
	if(x>=0 && x<LCD_WIDTH_PX && y>=0 && y<LCD_HEIGHT_PX) {
		color_t * base = y * LCD_WIDTH_PX + x + (color_t*)0xA8000000;
		alpha %= 32;
		*base = ((((color & 0xF81F) * alpha + (*base & 0xF81F) * (32-alpha)) >> 5) & 0xF81F) |
			((((color & 0x07E0) * alpha + (*base & 0x07E0) * (32-alpha)) >> 5) & 0x07E0);
	}
}*/

/*void CopySpriteNbitMaskedMirror(const unsigned char* data, int x, int y, int width, int height, const color_t* palette, color_t maskColor, unsigned int bitwidth,char mirror) //0 = normal, 1 = miroir verticale
{
   color_t* VRAM = (color_t*)0xA8000000;
   VRAM += (LCD_WIDTH_PX*y + x);
   int offset = 0;
   unsigned char buf;
   for(int j=y; j<y+height; j++)
   {
      int availbits = 0;
      if (mirror)offset = offset+width-1;
      for(int i=x; i<x+width;  i++)
      {
         if (!availbits)
         {
            if (!mirror)
                buf = data[offset++];
            else
                buf = data[offset--];
            availbits = 8;
         }
         color_t thiss = ((color_t)buf>>(8-bitwidth));
         color_t color = palette[thiss];
         if(color != maskColor && i >=0 && i<384)
         {
            *VRAM = color;
         }
         VRAM++;
         buf<<=bitwidth;
         availbits-=bitwidth;
      }
      VRAM += (LCD_WIDTH_PX-width);
      if (mirror)
        offset = offset+width+1;
   }
}

void CopySpriteNbitMaskedAlphaMirror(const unsigned char* data, int x, int y, int width, int height, const color_t* palette, color_t maskColor, unsigned int bitwidth,char alpha,char mirror)
{
   color_t* VRAM = (color_t*)0xA8000000;
   VRAM += (LCD_WIDTH_PX*y + x);
   alpha %= 32;
   int offset = 0;
   unsigned char buf;
   for(int j=y; j<y+height; j++)
   {
      int availbits = 0;
      if (mirror)offset = offset+width-1;
      for(int i=x; i<x+width;  i++)
      {
         if (!availbits)
         {
            if (!mirror)
                buf = data[offset++];
            else
                buf = data[offset--];
            availbits = 8;
         }
         color_t thiss = ((color_t)buf>>(8-bitwidth));
         color_t color = palette[thiss];
         if(color != maskColor && i >=0 && i<384)
         {
            *VRAM = ((((color & 0xF81F) * alpha + (*VRAM & 0xF81F) * (32-alpha)) >> 5) & 0xF81F) |
            ((((color & 0x07E0) * alpha + (*VRAM & 0x07E0) * (32-alpha)) >> 5) & 0x07E0);
         }
         //if (mirror) VRAM--;else
          VRAM++;
         buf<<=bitwidth;
         availbits-=bitwidth;
      }
      VRAM += (LCD_WIDTH_PX-width);
      if (mirror)offset = offset+width+2;
   }
}
*/
void BgColor(short color)
{
	int i, color32bits, *VRAM = GetVRAMAdress();
	color32bits = (color<<16) | color;
	for(i=LCD_WIDTH_PX*LCD_HEIGHT_PX/2 ; i ; i--)
		*(VRAM++) = color32bits;
}

int abs (int i)
{
    if (i>=0)
        return i;
    else
        return -i;
}

void SetPixelAlpha(int x, int y, color_t color, char alpha) {
    if(x>=0 && x<LCD_WIDTH_PX && y>=0 && y<LCD_HEIGHT_PX) {
        color_t * base = y * LCD_WIDTH_PX + x +(color_t*) 0xA8000000;;
        alpha %= 32;
        *base = ((((color & 0xF81F) * alpha + (*base & 0xF81F) * (32-alpha)) >> 5) & 0xF81F) |
            ((((color & 0x07E0) * alpha + (*base & 0x07E0) * (32-alpha)) >> 5) & 0x07E0);
    }
}

//Uses the Bresenham line algorithm
void drawLine(int x1, int y1, int x2, int y2, int color) {
    signed char ix;
    signed char iy;

    // if x1 == x2 or y1 == y2, then it does not matter what we set here
    int delta_x = (x2 > x1?(ix = 1, x2 - x1):(ix = -1, x1 - x2)) << 1;
    int delta_y = (y2 > y1?(iy = 1, y2 - y1):(iy = -1, y1 - y2)) << 1;

   plot(x1, y1, color);
    if (delta_x >= delta_y) {
        int error = delta_y - (delta_x >> 1);        // error may go below zero
        while (x1 != x2) {
            if (error >= 0) {
                if (error || (ix > 0)) {
                    y1 += iy;
                    error -= delta_x;
                }                           // else do nothing
         }                              // else do nothing
            x1 += ix;
            error += delta_y;
            plot(x1, y1, color);
        }
    } else {
        int error = delta_x - (delta_y >> 1);      // error may go below zero
        while (y1 != y2) {
            if (error >= 0) {
                if (error || (iy > 0)) {
                    x1 += ix;
                    error -= delta_y;
                }                           // else do nothing
            }                              // else do nothing
            y1 += iy;
            error += delta_x;
            plot(x1, y1, color);
        }
    }
}

void drawLineAlpha(int x1, int y1, int x2, int y2, int color,int alpha) {
    signed char ix;
    signed char iy;

    // if x1 == x2 or y1 == y2, then it does not matter what we set here
    int delta_x = (x2 > x1?(ix = 1, x2 - x1):(ix = -1, x1 - x2)) << 1;
    int delta_y = (y2 > y1?(iy = 1, y2 - y1):(iy = -1, y1 - y2)) << 1;

   SetPixelAlpha(x1, y1, color,alpha);
    if (delta_x >= delta_y) {
        int error = delta_y - (delta_x >> 1);        // error may go below zero
        while (x1 != x2) {
            if (error >= 0) {
                if (error || (ix > 0)) {
                    y1 += iy;
                    error -= delta_x;
                }                           // else do nothing
         }                              // else do nothing
            x1 += ix;
            error += delta_y;
            SetPixelAlpha(x1, y1, color,alpha);
        }
    } else {
        int error = delta_x - (delta_y >> 1);      // error may go below zero
        while (y1 != y2) {
            if (error >= 0) {
                if (error || (iy > 0)) {
                    x1 += ix;
                    error -= delta_y;
                }                           // else do nothing
            }                              // else do nothing
            y1 += iy;
            error += delta_x;
            SetPixelAlpha(x1, y1, color,alpha);
        }
    }
}


void BgColorAlpha(int couleur,int alpha)
{
	int i,j;
	for(i=0;i<=LCD_WIDTH_PX;i++)
	    for (j=0;j<=216;j++)
            SetPixelAlpha(i,j,couleur,alpha);
}


void FillRectangle(int x0,int y0, int x1, int y1, int couleur)
{
	int i,j;
	for(i=x0;i<x1;i++)
	{
        for (j=y0;j<y1;j++)
            if (i>=0 && i<=384 && j>=0 && j<=216)
                plot(i,j,couleur);
	}
}

void FillRectangleAlpha(int x0,int y0, int x1, int y1, int couleur,int alpha)
{
	int i,j;
	for(i=x0;i<x1;i++)
	{
        for (j=y0;j<y1;j++)
            if (i>=0 && i<=384 && j>=0 && j<=216)
                SetPixelAlpha(i,j,couleur,alpha);
	}
}