#include "mw_lcd.h"
#include "mw_lcd_draw.h"

void 
mw_lcd_draw_pixel(const uint8_t x, const uint8_t y, const uint8_t color)
{
	switch (color) {
		case 1:
			lcd_buf[y].Data[x/8] |= 1 << (x % 8);
			break;
		case 0:
			lcd_buf[y].Data[x/8] &= ~(1 << (x % 8));
			break;
		case 2:
			lcd_buf[y].Data[x/8] ^= 1 << (x % 8);
			break;
	}
}
#ifndef DIGITAL
//#define DEBUG_LCD
#endif
#ifdef DEBUG_LCD
#include <stdio.h>
void dump_lcd(int y1)
{
	uint8_t y,i;
	uint8_t x;
	for(y=y1,i=0;i<30 && y<=95;i++,y++) {
		for(x=0;x<96;x++) {
			printf("%c",(lcd_buf[y].Data[x/8] & (1 << (x % 8)))?'X':'.');
			//lcd_buf[y].Data[x/8]|=(1 << (x % 8));
		}
		printf("\n");
	}
}
#endif
void 
mw_lcd_draw_rect(const uint8_t x1, const uint8_t y1,const uint8_t x2, const uint8_t y2, const uint8_t color)
{
	uint8_t y;
	uint8_t x;
	if(color==1) {
		for(y=y1;y<=y2;y++) {
			for(x=x1;x<=x2;x++) {
				if((x&0x7)==0 && (x+7)<=x2) {
					lcd_buf[y].Data[x>>3]=0xff;
					x+=7;
				} else  {
					lcd_buf[y].Data[x>>3] |= (1 << (x&7));
				}
			}
		}
	} else if(color==0) {
		for(y=y1;y<=y2;y++) {
			for(x=x1;x<=x2;x++) {
				if((x&0x7)==0 && (x+7)<=x2) {
					lcd_buf[y].Data[x>>3]=0x00;
					x+=7;
				} else
					lcd_buf[y].Data[x>>3] &= ~(1 << (x & 7));
			}
		}
	} else {
		for(y=y1;y<=y2;y++) {
			for(x=x1;x<=x2;x++) {
				if((x&0x7)==0 && (x+7)<=x2) {
					lcd_buf[y].Data[x>>3]^=0xff;
					x+=7;
				} else
					lcd_buf[y].Data[x>>3] ^= (1 << (x & 7));
			}
		}
	}
}

void 
mw_lcd_draw_pixmap(const uint8_t x1, const uint8_t y1,const uint8_t fragmentsize,const uint8_t w, const uint8_t h, const uint8_t *bits, const uint8_t color)
{
	int y,y2,n;
	int nl;
	int s,si;
	int n0;
	int mask,tiny;
	const uint8_t *ptr,*fragmentptr;
#ifdef DEBUG_LCD
	//if(x1!=40)
	//	return;
	printf("(%i,%i), frag:%i, size(%i,%i) c:%i\n",x1,y1,fragmentsize,w,h,color),fflush(stdout);
	dump_lcd(y1);
#endif
	s=(x1&7);
	si=8-s;
	n0=x1>>3;
	y2=y1+h-1;
	nl=((x1+w-1)>>3);
	//mask=0xff>>(8-(w&7));
	tiny=(nl>n0 && w<=8)?1:0;
	if(nl==n0 && w==8)
		mask=0xff; // instead of 0x0
#ifdef DEBUG_LCD
	printf("s:%i, n0:%i y2:%i, nl:%i mask:0x%x\n",s,n0,y2,nl,mask),fflush(stdout);
	dump_lcd(y1);
#endif
	if(color==1) {
		if(nl>n0) {
			for(y=y1,fragmentptr=ptr=bits;y<=y2;y++,fragmentptr+=fragmentsize,ptr=fragmentptr) {
				lcd_buf[y].Data[n0]|=(*(ptr++))<<s;
				for(n=n0+1;n<nl;n++,ptr++)
					lcd_buf[y].Data[n]|=(((ptr[0])<<s)|((ptr[-1])>>si));
				if(tiny)
					lcd_buf[y].Data[nl]|=((ptr[-1])>>si);
				else
					lcd_buf[y].Data[nl]|=((((ptr[0]&mask)<<s)|((ptr[-1])>>si)));
			}
		} else {
			for(y=y1,fragmentptr=ptr=bits;y<=y2;y++,fragmentptr+=fragmentsize,ptr=fragmentptr) 
				lcd_buf[y].Data[n0]|=(((*ptr)&mask)<<s);
		}
	} else {
		if(nl>n0) {
			for(y=y1,fragmentptr=ptr=bits;y<=y2;y++,fragmentptr+=fragmentsize,ptr=fragmentptr) {
				lcd_buf[y].Data[n0]&=~((*(ptr++))<<s);
				for(n=n0+1;n<nl;n++,ptr++)
					lcd_buf[y].Data[n]&=~((((ptr[0])<<s)|((ptr[-1])>>si)));
				if(tiny)
					lcd_buf[y].Data[nl]&=~(((ptr[-1])>>si));
				else
					lcd_buf[y].Data[nl]&=~(((((ptr[0]&mask)<<s)|((ptr[-1])>>si))));
			}
		} else {
			for(y=y1,fragmentptr=ptr=bits;y<=y2;y++,fragmentptr+=fragmentsize,ptr=fragmentptr) 
				lcd_buf[y].Data[n0]&=~(((*ptr)&mask)<<s);
		}
	}
#ifdef DEBUG_LCD
	printf("After:\n");
	dump_lcd(y1);
#endif
}