#include <os.h>
#include "output.h"
#include "charmap_8x12.h"

#define STR_SELECTION_COLOR (has_colors?0b011101111111111:0xA)

inline void setPixelBuf(void* scrbuf, unsigned x, unsigned y, uint16_t color){
    if(x < SCREEN_WIDTH && y < SCREEN_HEIGHT){
        if(!has_colors){
            uint8_t* p = (uint8_t*) scrbuf + (x >> 1) + (y << 7) + (y << 5);
            if(x%2)
                *p = (*p & 0xF0) | color;
            else
                *p = (*p & 0x0F) | color << 4;
            //*p = (x % 2) ? ((*p & 0b11110000) | color) : ((*p & 0b00001111) | (color << 4));
            //*p = (*p & (0b1111 << (x%2)*4)) | (color << (1 - (x%2))*4);
        }
        else{
            uint16_t* p = (uint16_t*) (scrbuf + (x << 1) + (y << 9) + (y << 7));//(x << 1) + (y << 9) + (y << 7);
            *p = color;
        }
    }
}

void putChar(void* scrbuf, unsigned x, unsigned y, uint8_t chr){
	int i, j, pixelOn;
	for(i = 0; i < CHAR_HEIGHT; i++){
		for(j = 0; j < CHAR_WIDTH; j++){
			pixelOn = charMap_ascii[chr][i] << j ;
			pixelOn = pixelOn & 0b10000000;
			if(pixelOn){
				setPixelBuf(scrbuf, x + j, y + i, 0);
			}
			//else{
			//	setPixelBuf(scrbuf, x + j, y + i, has_colors ? 0xFFFF : 0xF);
			//}
		}
	}
}

void putCharColor(void* scrbuf, unsigned x, unsigned y, uint8_t chr, uint16_t chrcolor, uint16_t bgcolor){
	int i, j, pixelOn;
	for(i = 0; i < CHAR_HEIGHT; i++){
		for(j = 0; j < CHAR_WIDTH; j++){
			pixelOn = charMap_ascii[chr][i] << j ;
			pixelOn = pixelOn & 0x80 ;
			setPixelBuf(scrbuf, x + j, y + i, pixelOn ? chrcolor : bgcolor);
		}
	}
}

void dispString(void* scrbuf, unsigned x, unsigned y, const char* message){
	int l = strlen(message);
	int i;
	unsigned line=0, col=0;
	for (i = 0; i<l; i++){
	    if(message[i] == '\n'){
	        col = 0;
	        ++line;
	    }
	    else if(message[i] == '\t')
	        col += TAB_WIDTH - (col % TAB_WIDTH);
	    else{
            putChar(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, message[i]);
            ++col;
	    }
		if (x+col*CHAR_WIDTH >= SCREEN_WIDTH){
			col = 0;
			++line;
		}
		if(y + line*CHAR_HEIGHT >= SCREEN_HEIGHT)//damit nicht speicher hinter screenbuffer überschrieben wird
			break;
	}
}

void dispStringColor(void* scrbuf, unsigned x, unsigned y, char* message, uint16_t chrcolor, uint16_t bgcolor){
	char* p;
	unsigned line=0, col=0;
	for(p = message; *p; ++p){
	    if(*p == '\n'){
            putCharColor(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, ' ', chrcolor, bgcolor);//disp colored blank
	        col = 0;
	        ++line;
	    }
	    else if(*p == '\t'){
            while(TAB_WIDTH - col % TAB_WIDTH){
                putCharColor(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, ' ', chrcolor, bgcolor);
                col++;
            }
	    }
	    else{
            putCharColor(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, *p, chrcolor, bgcolor);
            ++col;
	    }
		if (x+col*CHAR_WIDTH >= SCREEN_WIDTH){
			col = 0;
			++line;
		}
		if(y + line*CHAR_HEIGHT >= SCREEN_HEIGHT)
			break;
	}
}

void dispStringWithSelection(void* scrbuf, unsigned x, unsigned y, char* message, int selectionstart, int selectionend){
    int l = strlen(message);
	int i;
	unsigned line=0, col=0;
	for (i = 0; i<l; i++){
        if(message[i] == '\n'){
            if(i >= selectionstart && i < selectionend)
                putCharColor(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, ' ', 0, STR_SELECTION_COLOR);//disp colored blank
            col = 0;
            ++line;
        }
        else if(message[i] == '\t'){
            if(i >= selectionstart && i < selectionend){
                while(TAB_WIDTH - (col % TAB_WIDTH)){
                    putCharColor(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, ' ', 0, STR_SELECTION_COLOR);
                    col++;
                }
            }
            else
                col += TAB_WIDTH - (col % TAB_WIDTH);
        }
        else{
            if(i >= selectionstart && i < selectionend)
                putCharColor(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, message[i], has_colors ? 0xFFFF : 0xF, STR_SELECTION_COLOR);
            else
                putChar(scrbuf, x+col*CHAR_WIDTH, y+line*CHAR_HEIGHT, message[i]);
            ++col;
        }

        if (x+col*CHAR_WIDTH >= SCREEN_WIDTH){
            col = 0;
            ++line;
        }
        if(y + line*CHAR_HEIGHT >= SCREEN_HEIGHT)
            break;
	}
}

void dispCursor(uint8_t* scrbuf, int offset, char* displinep){//offset=where is cursor relative to displinep
    int row=0, col=0;
    int i;
    for(i=0; i<offset; i++){
        switch(displinep[i]){
            case '\n':
                row++;
                col=0;
            break;
            case '\t':
                col += TAB_WIDTH - (col % TAB_WIDTH);
            break;
            default: col++;
        }
        if(col >= SCREEN_WIDTH/CHAR_WIDTH){
            row++;
            col = 0;
        }
    }
    for(i=0; i<CHAR_HEIGHT; i++)
        setPixelBuf(scrbuf, col*CHAR_WIDTH, row*CHAR_HEIGHT + i, 0x0);
}

inline void dispHorizLine(uint8_t* scrbuf, int x, int y, int l, int color){
    while(l--)
        setPixelBuf(scrbuf, x++, y, color);
}

inline void dispVertLine(uint8_t* scrbuf, int x, int y, int l, int color){
    while(l--)
        setPixelBuf(scrbuf, x, y++, color);
}

void dispRect(uint8_t* scrbuf, int x, int y, int w, int h, int color){
    dispHorizLine(scrbuf, x, y, w, color);
    dispHorizLine(scrbuf, x, y+h-1, w, color);
    dispVertLine(scrbuf, x, y+1, h-2, color);
    dispVertLine(scrbuf, x+w-1, y+1, h-2, color);
}

void filledRect(uint8_t* scrbuf, int x, int y, int w, int h, int color){
    while(h--)
        dispHorizLine(scrbuf, x, y++, w, color);
}