#include <os.h>
#include "screen.h"
#include "charmap.h"

#define SCREEN_BASE_PTR		0xC0000010 

unsigned short int* addr=0;

unsigned short int* getScreen()
{	return addr;
}

void initScreen()
{	addr	=*(unsigned short int**)SCREEN_BASE_PTR;
}

unsigned short int rgb2data(char r, char g, char b)
{	return ((r&0b11111000)<<8)|((g&0b11111100)<<3)|(b>>3);
}

void setBufPixel(unsigned short int* buf, unsigned int x, unsigned int y, unsigned int color)
{	while(x<0)
	{	x+=SCREEN_WIDTH;
		y--;
	}
	while(x>=SCREEN_WIDTH)
	{	x-=SCREEN_WIDTH;
		y++;
	}
	if(y >= 0 && y < SCREEN_HEIGHT)
		buf[y*SCREEN_WIDTH+x]=color;
}


void setPixel(unsigned int x, unsigned int y, unsigned int color)
{	setBufPixel(addr,x,y,color);
}

void getBufPixelRGB(unsigned short int* buf, unsigned int x, unsigned int y, int*r, int*g, int*b)
{	if(x >= 0 && x < SCREEN_WIDTH && y >= 0 && y < SCREEN_HEIGHT)
	{	int color = buf[y*SCREEN_WIDTH+x];
		*r=(color&0b1111100000000000)>>8;
		*g=(color&0b0000011111100000)>>3;
		*b=(color&0b0000000000011111)<<3;
	}
}

void getPixelRGB(unsigned int x, unsigned int y, int*r, int*g, int*b)
{	getBufPixelRGB(addr,x,y,r,g,b);
}

void setBufPixelRGB(unsigned short int* buf, unsigned int x, unsigned int y, int r, int g, int b)
{	if(r<0 || g<0 || b<0)
	{	getBufPixelRGB(buf,x,y,&r,&g,&b);
		if(r+g+b>=440)
		{	r=0; g=0; b=0;	}
		else
		{	r=255; g=255; b=255;	}
	}
	setBufPixel(buf, x, y, rgb2data(r,g,b));
}

void setPixelRGB(unsigned int x, unsigned int y, int r, int g, int b)
{	setBufPixelRGB(addr,x,y,r,g,b);
}

void drawBufLineHorizRGB(unsigned short int* buf, unsigned int y, int x1, int x2, int r, int g, int b, int s)
{	int i = x1;
	if(!s) s=1;
	while(i<=x2)
	{	setBufPixelRGB(buf,i,y,r,g,b);
		i+=s;
	}
}

void drawLineHorizRGB(unsigned int y, int x1, int x2, int r, int g, int b, int s)
{	drawBufLineHorizRGB(addr,y,x1,x2,r,g,b,s);
}

void drawBufLineVertRGB(unsigned short int* buf, unsigned int x, int y1, int y2, int r, int g, int b, int s)
{	int i = y1;
	if(!s) s=1;
	while(i<=y2)
	{	setBufPixelRGB(buf,x,i,r,g,b);
		i+=s;
	}
}

void drawLineVertRGB(unsigned int x, int y1, int y2, int r, int g, int b, int s)
{	drawBufLineVertRGB(addr,x,y1,y2,r,g,b,s);
}

void drawBufFullHorizRGB(unsigned short int* buf, unsigned int y, int r, int g, int b, int s)
{	drawBufLineHorizRGB(buf,y,0,SCREEN_WIDTH-1,r,g,b,s);
}

void drawFullHorizRGB(unsigned int y, int r, int g, int b, int s)
{	drawBufFullHorizRGB(addr,y,r,g,b,s);
}

void drawBufFullVertRGB(unsigned short int* buf, unsigned int x, int r, int g, int b, int s)
{	drawBufLineVertRGB(buf,x,0,SCREEN_HEIGHT-1,r,g,b,s);
}

void drawFullVertRGB(unsigned int x, int r, int g, int b, int s)
{	drawBufLineVertRGB(addr,x,0,SCREEN_HEIGHT-1,r,g,b,s);
}

void drawBufLineBoxRGB(unsigned short int* buf, int x1, int y1, int x2, int y2, int r, int g, int b, int s)
{	if(!s) s=1;
	drawBufLineHorizRGB(buf,y1,x1,x2,r,g,b,s);
	drawBufLineVertRGB(buf,x1,y1+s,y2,r,g,b,s);
	drawBufLineVertRGB(buf,x2,y1+s,y2,r,g,b,s);
	drawBufLineHorizRGB(buf,y2,x1+s,x2-s,r,g,b,s);
}

void drawLineBoxRGB(int x1, int y1, int x2, int y2, int r, int g, int b, int s)
{	drawBufLineBoxRGB(addr,x1,y1,x2,y2,r,g,b,s);
}

void drawBufFullBoxRGB(unsigned short int* buf, unsigned int x1, unsigned int y1, unsigned int x2, unsigned int y2, int r, int g, int b)
{	unsigned int m = max(y1,y2);
	unsigned int i = min(y1,y2);
	m=min(m,SCREEN_HEIGHT-1);
	i=min(i,SCREEN_HEIGHT-1);
	while(i<=m)
	{	drawBufLineHorizRGB(buf,i,x1,x2,r,g,b,1);
		i++;
	}
}

void drawFullBoxRGB(unsigned int x1, unsigned int y1, unsigned int x2, unsigned int y2, int r, int g, int b)
{	drawBufFullBoxRGB(addr,x1,y1,x2,y2,r,g,b);
}

void drawBufImg(unsigned short int* buf, unsigned int x, unsigned int y, unsigned short int* image, unsigned int w, unsigned int h)
{	int i,j;
	for(i=0;i<h;i++)
		if(y+i<SCREEN_HEIGHT-1) memcpy(buf+(y+i)*SCREEN_WIDTH+x,image+i*w,w*2);
		else if(y+i==SCREEN_HEIGHT-1) memcpy(buf+(y+i)*SCREEN_WIDTH+x,image+i*w,min(w,SCREEN_WIDTH-x)*2);
}

void drawImg(unsigned int x, unsigned int y, unsigned short int* image, unsigned int w, unsigned int h)
{	drawBufImg(addr,x,y,image,w,h);
}

void drawBufImgRGB(unsigned short int* buf, unsigned int x, unsigned int y, unsigned char* image, unsigned int w, int h)
{	int i,j;
	int sh=h;
	if(h<0) sh=-h;
	unsigned short int* image2 = malloc(w*sh*2);
	for(i=0;i<sh;i++)
	{	for(j=0;j<w;j++)
		{	
			if(h>0)	image2[w*i+j]=rgb2data(image[(w*i+j)*3],image[(w*i+j)*3+1],image[(w*i+j)*3+2]);
			else	image2[w*(sh-i-1)+j]=rgb2data(image[(w*i+j)*3],image[(w*i+j)*3+1],image[(w*i+j)*3+2]);
		}
	}
	drawBufImg(buf,x,y,image2,w,sh);
	free(image2);
}

void drawBufImgBGR(unsigned short int* buf, unsigned int x, unsigned int y, unsigned char* image, unsigned int w, int h)
{	int i,j;
	int sh=h;
	if(h<0) sh=-h;
	unsigned short int* image2 = malloc(w*sh*2);
	for(i=0;i<sh;i++)
	{	for(j=0;j<w;j++)
		{	
			if(h>0)	image2[w*i+j]=rgb2data(image[(w*i+j)*3+2],image[(w*i+j)*3+1],image[(w*i+j)*3]);
			else	image2[w*(sh-i-1)+j]=rgb2data(image[(w*i+j)*3+2],image[(w*i+j)*3+1],image[(w*i+j)*3]);
		}
	}
	drawBufImg(buf,x,y,image2,w,sh);
	free(image2);
}

void drawImgRGB(unsigned int x, unsigned int y, unsigned char* image, unsigned int w, unsigned int h)
{	drawBufImgRGB(addr,x,y,image,w,h);
}


void drawBufImgBox(unsigned short int* buf, unsigned int x1, unsigned int y1, unsigned int x2, unsigned int y2, unsigned short int* image, int w, int h)
{	unsigned short int color;
	unsigned char* pimage = image;
	if((int)image%2) pimage=(int)pimage-1;
	x1=min(x1,SCREEN_WIDTH-1);
	x2=min(x2,SCREEN_WIDTH-1);
	y1=min(y1,SCREEN_HEIGHT-1);
	y2=min(y2,SCREEN_HEIGHT-1);
	unsigned int ymax = max(y1,y2);
	unsigned int ymin = min(y1,y2);
	unsigned int xmax = max(x1,x2);
	unsigned int xmin = min(x1,x2);
	if(w>h && w>xmax-xmin+1)
	{	int t = (xmax-xmin+1)*h/w;
		t=ymax-ymin-t;
		ymin+=t/2;
		ymax-=t/2;
		if(t%2) ymin++;
	}
	else if(h>w && h>ymax-ymin+1)
	{	int t = (ymax-ymin+1)*w/h;
		t=xmax-xmin-t;
		xmin+=t/2;
		xmax-=t/2;
		if(t%2) xmin++;
	}
	else
	{	unsigned int d;
		if(w<xmax-xmin+1)
		{	d = xmax-xmin+1-w;
			xmin+=d/2;
			xmax-=d/2;
			if(d%2) xmin++;
		}
		if(h<ymax-ymin+1)
		{	d = ymax-ymin+1-h;
			ymin+=d/2;
			ymax-=d/2;
			if(d%2) ymin++;
		}
	}
	if(w<=xmax-xmin+1 && h<=ymax-ymin+1)
		drawBufImg(buf,xmin,ymin,image,w,h);
	else
	{	unsigned x, y, dx, dy;
		for(y=ymin;y<=ymax;y++)
			for(x=xmin;x<=xmax;x++)
			{	dx = (x-xmin)*w/(xmax-xmin+1);
				dy = (y-ymin)*h/(ymax-ymin+1);
				if((int)image%2)
					color=(pimage[(dy*w+dx)*2]<<8)|(pimage[(dy*w+dx)*2+1]);
				else
					color=image[dy*w+dx];
				setBufPixel(buf,x,y,color);
			}
	}
}

void drawImgBox(unsigned int x1, unsigned int y1, unsigned int x2, unsigned int y2, unsigned short int* image, int w, int h)
{	drawBufImgBox(addr,x1,y1,x2,y2,image,w,h);
}

void putBufCharRGB(unsigned short int* buf, int x, int y, char ch, int r, int g, int b)
{
  int i, j, pixelOn;
  for(i = 0; i < CHAR_HEIGHT; i++)  
  {
    for(j = 0; j < CHAR_WIDTH; j++) 
    {
      pixelOn = charMap_ascii[(unsigned char)ch][i] << j ;  
      pixelOn = pixelOn & 0x80 ;  
      if (pixelOn) {
		setBufPixelRGB(buf, x + j, y + i, r,g,b); 
      } 
    }
  } 
}

void putCharRGB(int x, int y, char ch, int r, int g, int b)
{	putBufCharRGB(addr,x,y,ch,r,g,b);
}

void drwBufStrRGB(unsigned short int* buf, int x, int y, char* str, int ret, int r, int g, int b)
{ int l = strlen(str);
  int i;
  int stop=0;
  for (i = 0; i < l && !stop; i++) {
    if (str[i] == 0x0A) {
      if(ret)
      { x = 0;
        y += CHAR_HEIGHT;
      }
      else
      { putBufCharRGB(buf,x,y,' ',r,g,b); 
        x += CHAR_WIDTH;
      }
    } else {
      putBufCharRGB(buf,x, y, str[i],r,g,b);
      x += CHAR_WIDTH;
    }
    if (x > SCREEN_WIDTH-CHAR_WIDTH)
    { if(ret)
      { x = 0;
        y += CHAR_HEIGHT;
      }
      else
        stop=1;
    }
  }
}

void drwStrRGB(int x, int y, char* str, int ret, int r, int g, int b)
{	drwBufStrRGB(addr,x,y,str,ret,r,g,b);
}