#include <os.h>
#include "screen.h"
#include "charmap.h"
#define SCREEN_CONTRAST_ADDR 0x900F0020
#define SCREEN_BASE_ADDR 0xa4000100

#define SCREEN_BASE_PTR		0xC0000010 
#define SCREEN_MODE_ADDR	0xC000001C
#define SCREEN_INT_ADDR		0xC0000020

#define SCREEN4_SIZE	SCREEN_WIDTH*SCREEN_HEIGHT/2
#define SCREEN16_SIZE	SCREEN_WIDTH*SCREEN_HEIGHT*2

int mode_bits=4;
unsigned char* addr=SCREEN_BASE_ADDR;

int isScrReady()
{return (*(volatile unsigned*) SCREEN_INT_ADDR)&2;
}

void setScrDirty()
{	(*(volatile unsigned*) SCREEN_INT_ADDR)=0b100;
}

void setScreen(unsigned char* buf)
{	addr=buf;
	*(volatile unsigned*) SCREEN_BASE_PTR = buf;
}

void startScrMode16()
{	int mode = *(volatile unsigned*) SCREEN_MODE_ADDR;
	mode = mode&~0b1110;
	mode = mode|0b1000;
	*(volatile unsigned*) SCREEN_MODE_ADDR = mode;
	mode_bits=16;
}

void endScrMode16()
{	if(mode_bits==16)
	{	int mode = *(volatile unsigned*) SCREEN_MODE_ADDR;
		mode = mode&~0b100001110;
		mode = mode|0b0100;
		*(volatile unsigned*) SCREEN_MODE_ADDR = mode;
		mode_bits=4;
		*(volatile unsigned*) SCREEN_BASE_PTR = SCREEN_BASE_ADDR;
		addr=SCREEN_BASE_ADDR;
	}
} 

void switchScrOffOn(int s)
{	int mask = 0b100000000001;
	int mode = *(volatile unsigned*) SCREEN_MODE_ADDR;
	if(s)	mode |= mask;
	else	mode &= ~mask;
	*(volatile unsigned*) SCREEN_MODE_ADDR = mode;	
	return mode&mask;
}

void switchRGB(int brg)
{ int mode = *(volatile unsigned*) SCREEN_MODE_ADDR;
  int mask = 0b00100000000;
  if(!brg)
  {	brg=1;
	mode |= mask;
  }
  else
  {	brg=0;
	mode &= ~mask;
  }
  *(volatile unsigned*) SCREEN_MODE_ADDR = mode;
}

unsigned int getContrast()
{ return *(volatile unsigned*) SCREEN_CONTRAST_ADDR;
}

void setContrast(unsigned int level)
{ *(volatile unsigned*) SCREEN_CONTRAST_ADDR=level;
}

void setBufPixel(unsigned char* buf, int x, int y, unsigned int color)
{	unsigned char lowcolor = color>>1;
	if(x >= 0 && x < SCREEN_WIDTH && y >= 0 && y < SCREEN_HEIGHT)
	{	if(mode_bits==4)
			buf[(y*SCREEN_WIDTH+x)/2]=(x&1)? (buf[(y*SCREEN_WIDTH+x)/2]&0xF0)|lowcolor : (buf[(y*SCREEN_WIDTH+x)/2]&0x0F)|(lowcolor<<4);
		else
		{	buf[(y*SCREEN_WIDTH+x)*2]	=(~lowcolor)<<1;
			if(!(color&1) && color)
				buf[(y*SCREEN_WIDTH+x)*2+1]=(~(lowcolor-1))<<3;
			else
				buf[(y*SCREEN_WIDTH+x)*2+1]=(~lowcolor)<<3;
		}
	}
}

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

void drwVert(int x, int y1, int y2, int color)
{	int m = max(y1,y2);
	int i = min(y1,y2);
	while(i<=m)
	{	setPixel(x,i,color);
		i++;
	}
}

void drwHoriz(int y, int x1, int x2, int color)
{	int m = max(x1,x2);
	int i = min(x1,x2);
	while(i<=m)
	{	setPixel(i,y,color);
		i++;
	}
}

void drwFullHoriz(int y)
{	drwHoriz(y, 0, SCREEN_WIDTH-1, 0x00);
}

void drwBox(int x1, int y1, int x2, int y2, int color)
{	drwHoriz(y1,x1,x2,color);
	drwHoriz(y2,x1,x2,color);
	drwVert(x1,y1,y2,color);
	drwVert(x2,y1,y2,color);
}

void dispBufIMG(unsigned char* buf, int xoff, int yoff, char* img, int width, int height, float inc)
{	int dwidth=width, dheight=height;
	int data_x=0, data_y=0;
	unsigned int x = 0, y = 0;
	float i, j;
	if(xoff < 0){
		dwidth = dwidth + xoff;
		data_x = (int)(-xoff*inc);
		xoff = 0;
	}
	if(yoff < 0){
		dheight = dheight + yoff;
		data_y = (int)(-yoff*inc);	
		yoff = 0;
	}
	for(i=0, x=0; (int)i < dwidth && x < SCREEN_WIDTH; i+= inc, x++)
		for(j=0, y=0; (int)j < dheight && y < SCREEN_HEIGHT; j+= inc, y++)
			setBufPixel(buf, xoff + x, yoff + y, img[((int)j+data_y)*width+(int)i+data_x]);
}

void dispIMG(int xoff, int yoff, char* img, int width, int height, float inc)
{	dispBufIMG(addr,xoff,yoff,img,width,height,inc);
}


void putBufChar(unsigned char* buf, int x, int y, char ch)
{
  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) {
        setBufPixel(buf,x + j, y + i, 0); 
      } else {
        setBufPixel(buf,x + j, y + i, 31);
      } 
    }
  } 
}

void putChar(int x, int y, char ch)
{	putBufChar((unsigned char *) addr,x,y,ch);
}

void drwBufStr(unsigned char* buf, int x, int y, char* str, int ret)
{ 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
      { putBufChar(buf, x,y, ' '); 
        x += CHAR_WIDTH;
      }
    } else {
      putBufChar(buf, x, y, str[i]);
      x += CHAR_WIDTH;
    }
    if (x >= SCREEN_WIDTH-CHAR_WIDTH && ret || x>=SCREEN_WIDTH)
    { if(ret)
      { x = 0;
        y += CHAR_HEIGHT;
      }
      else
        stop=1;
    }
  }
}


void drwStr(int x, int y, char* str, int ret)
{ drwBufStr((unsigned char *) addr,x,y,str,ret);
}

void clrBuf(unsigned char* buf)
{	if(mode_bits==4)	memset(buf,0xFF,SCREEN4_SIZE);
	else			memset(buf,0,SCREEN16_SIZE);
}

void clrScr()
{	clrBuf((unsigned char *) addr);
}

void clrBufBox(unsigned char* buf, int x,int y, int w, int h)
{	int i,j;
	if(x<0) { w=w+x; x=0; }
	if(y<0) { h=h+y; y=0; }
	if(y+h>SCREEN_HEIGHT) h=SCREEN_HEIGHT-y;
	if(x+w>SCREEN_WIDTH) w=SCREEN_WIDTH-x;
	for(j = y; j < y+h; j ++)
	{	i=x;
		if(mode_bits==4)
		{	if(i&1)
			{	setBufPixel(buf,i,j,0xF);
				i++;
			}
			memset(buf+(j*SCREEN_WIDTH+i)/2,0xFF,w/2);
			if(w%2)	setBufPixel(buf,i+w-1,j,0xF);
		}
		else
			memset(buf+(j*SCREEN_WIDTH+i)*2,0,w*2);
		
	}
}

void clrBox(int x,int y, int w, int h)
{	clrBufBox(addr,x,y,w,h);
}

void refreshScr(unsigned char* buf)
{	memcpy(addr,buf,SCREEN_WIDTH*SCREEN_HEIGHT*mode_bits/8);
}