File content as of revision 0:4395705c8647:

// General Drivers
#include "mbed.h"

#include "ClockControl.h"

// QVGA LCD Drivers

////////
#include "font_courier.h"

// LCD Control
DigitalOut lcd_rst(p13);
DigitalOut lcd_bl(p14);
DigitalOut lcd_rs(p15);
DigitalOut lcd_cs(p16);
DigitalOut lcd_rd(p17);
DigitalOut lcd_wr(p18);

BusInOut lcd_db(p5, p6, p7, p8, p9, p10, p11, p12);

// Touch Screen Control
/*
#define TC_CS_PORT        &PORTB
#define TC_CS_PIN        4

#define TC_PEN_PORT        &PORTB
#define TC_PEN_PINP        PINB
#define TC_PEN_PIN        2

#define MOSI_PORT        &PORTB
#define MOSI_PIN        5

#define SCK_PORT        &PORTB
#define SCK_PIN            7
*/

//touch screen LCD configuration
//#define TS_ORN_PORTRAIT

#ifdef TS_ORN_PORTRAIT
#define TS_SIZE_X                    240
#define TS_SIZE_Y                    320
#define TS_VAL_ENTRY_MOD            0x0030
#define TS_INS_GRAM_ADX                TS_INS_GRAM_HOR_AD
#define TS_INS_GRAM_ADY                TS_INS_GRAM_VER_AD
#define TS_INS_START_ADX               TS_INS_HOR_START_AD
#define TS_INS_END_ADX               TS_INS_HOR_END_AD
#define TS_INS_START_ADY               TS_INS_VER_START_AD
#define TS_INS_END_ADY               TS_INS_VER_END_AD
#else
#define TS_SIZE_X                    320
#define TS_SIZE_Y                    240
#define TS_VAL_ENTRY_MOD            0x0028
#define TS_INS_GRAM_ADX                TS_INS_GRAM_VER_AD
#define TS_INS_GRAM_ADY                TS_INS_GRAM_HOR_AD
#define TS_INS_START_ADX               TS_INS_VER_START_AD
#define TS_INS_END_ADX               TS_INS_VER_END_AD
#define TS_INS_START_ADY               TS_INS_HOR_START_AD
#define TS_INS_END_ADY               TS_INS_HOR_END_AD
#endif

//pen status type definition
typedef enum
{
    PST_NOTFOUND,
    PST_DOWN,
    PST_HOLD,
    PST_UP
} pstatus_t;

//#define so_pos_t        short

#ifndef so_pos_t
typedef short so_pos_t;
#endif

//////

#define TS_INS_START_OSC            0x00 //data read at this instruction should be 0x0789 --> use for test connection
#define TS_INS_DRIV_OUT_CTRL        0x01
#define TS_INS_DRIV_WAV_CTRL        0x02
#define TS_INS_ENTRY_MOD            0x03
#define TS_INS_RESIZE_CTRL            0x04
#define TS_INS_DISP_CTRL1            0x07
#define TS_INS_DISP_CTRL2            0x08
#define TS_INS_DISP_CTRL3            0x09
#define TS_INS_DISP_CTRL4            0x0A
#define TS_INS_RGB_DISP_IF_CTRL1    0x0C
#define TS_INS_FRM_MARKER_POS        0x0D
#define TS_INS_RGB_DISP_IF_CTRL2    0x0F
#define TS_INS_POW_CTRL1            0x10
#define TS_INS_POW_CTRL2            0x11
#define TS_INS_POW_CTRL3            0x12
#define TS_INS_POW_CTRL4            0x13
#define TS_INS_GRAM_HOR_AD            0x20
#define TS_INS_GRAM_VER_AD            0x21
#define TS_INS_RW_GRAM                0x22
#define TS_INS_POW_CTRL7            0x29
#define TS_INS_FRM_RATE_COL_CTRL    0x2B
#define TS_INS_GAMMA_CTRL1            0x30
#define TS_INS_GAMMA_CTRL2            0x31
#define TS_INS_GAMMA_CTRL3            0x32
#define TS_INS_GAMMA_CTRL4            0x35 
#define TS_INS_GAMMA_CTRL5            0x36
#define TS_INS_GAMMA_CTRL6            0x37
#define TS_INS_GAMMA_CTRL7            0x38
#define TS_INS_GAMMA_CTRL8            0x39
#define TS_INS_GAMMA_CTRL9            0x3C
#define TS_INS_GAMMA_CTRL10            0x3D
#define TS_INS_HOR_START_AD            0x50
#define TS_INS_HOR_END_AD            0x51
#define TS_INS_VER_START_AD            0x52
#define TS_INS_VER_END_AD            0x53
#define TS_INS_GATE_SCAN_CTRL1        0x60
#define TS_INS_GATE_SCAN_CTRL2        0x61
#define TS_INS_GATE_SCAN_CTRL3        0x6A
#define TS_INS_PART_IMG1_DISP_POS    0x80
#define TS_INS_PART_IMG1_START_AD    0x81
#define TS_INS_PART_IMG1_END_AD        0x82
#define TS_INS_PART_IMG2_DISP_POS    0x83
#define TS_INS_PART_IMG2_START_AD    0x84
#define TS_INS_PART_IMG2_END_AD        0x85
#define TS_INS_PANEL_IF_CTRL1        0x90
#define TS_INS_PANEL_IF_CTRL2        0x92
#define TS_INS_PANEL_IF_CTRL3        0x93
#define TS_INS_PANEL_IF_CTRL4        0x95
#define TS_INS_PANEL_IF_CTRL5        0x97
#define TS_INS_PANEL_IF_CTRL6        0x98

#define TS_COL_RED                    0x001F
#define TS_COL_GREEN                0x07E0
#define TS_COL_BLUE                    0xF800
#define TS_COL_YELLOW                0x07FF
#define TS_COL_PURPLE                0xF81F
#define TS_COL_BLACK                0x0000
#define TS_COL_WHITE                0xFFFF

#define ts_pos_t                    int

typedef enum
{
    TS_MODE_NORMAL,
    TS_MODE_INVERSE,
    TS_MODE_FULL,
} ts_mode_t;

void TSLCDOutIns(unsigned short ins); //write instruction to LCD
void TSLCDOutDat(unsigned short dat); //write data to LCD
void TSLCDOutDat2(unsigned char dath,unsigned char datl); //write data to LCD
unsigned short TSLCDInIns(void); //read data from LCD
unsigned short TSLCDInDat(void); //read data from LCD

void TSLCDRst(void); //pulse reset signal to LCD
void TSLCDInit(void); //initial LCD
void TSLCDShowPic2(ts_pos_t sx,ts_pos_t ex,ts_pos_t sy,ts_pos_t ey,const unsigned short *pic,ts_mode_t mode);
//show picture from code memory with specific size
void TSLCDFillRect(ts_pos_t sx,ts_pos_t ex,ts_pos_t sy,ts_pos_t ey,unsigned short color,ts_mode_t mode); //draw a rectangular
void TSLCDFillCirc(ts_pos_t cx,ts_pos_t cy,ts_pos_t rad,unsigned short color, ts_mode_t mode); //draw a circle
void TSLCDSetMargins(ts_pos_t xl,ts_pos_t xr,ts_pos_t yu,ts_pos_t yl); //set margins for FillRect,FillCirc
void TSLCDSetMarginsDefault(void); //reset margins to default value

void TSLCDSetFontColor(unsigned short color); //set text's color
void TSLCDSetBackColor(unsigned short color); //set back color for TS_MODE_FULL
void TSLCDSetOffset(ts_pos_t x,ts_pos_t y); //set LCD offset for character display
void TSLCDPrintStr(unsigned char line,unsigned char column,char *str,ts_mode_t mode); //print string on LCD
void TSLCDPrintTxt(unsigned char line,unsigned char column,const char *txt,ts_mode_t mode); //print text from code memory
void TSLCDPrintCh(unsigned char line,unsigned char column,char c,ts_mode_t mode); //print a character on LCD



#define FONT_BIT_WIDTH 8
#define FONT_BYTE_HIGHT 2
#define FONT_SIZE FONT_BIT_WIDTH*FONT_BYTE_HIGHT

#define FONT_WIDTH FONT_BIT_WIDTH
#define FONT_HEIGHT FONT_BYTE_HIGHT*8

void TSLCDCharDisp(char charactor,ts_pos_t sx,ts_pos_t sy,ts_mode_t mode); //low level function to print a character on LCD

unsigned char char_buf[FONT_BIT_WIDTH][FONT_BYTE_HIGHT];

void buf_store(unsigned char charactor);

unsigned short font_color;
unsigned short back_color;
ts_pos_t offsetx, offsety;
ts_pos_t ts_margin_xl = 0;
ts_pos_t ts_margin_xr = TS_SIZE_X - 1;
ts_pos_t ts_margin_yu = 0;
ts_pos_t ts_margin_yl = TS_SIZE_Y - 1;

#define TSLCDGetMarginXl()            ts_margin_xl
#define TSLCDGetMarginXr()            ts_margin_xr
#define TSLCDGetMarginYu()            ts_margin_yu
#define TSLCDGetMarginYl()            ts_margin_yl

void TSLCDSetFontColor(unsigned short color) //set text's color
{
    font_color = color;
}

void TSLCDSetBackColor(unsigned short color) //set back color for TS_MODE_FULL
{
    back_color = color;
}

void TSLCDOutDat(unsigned short dat) //write data to LCD
{
    lcd_rs.write(1);

    lcd_rd.write(1);
    lcd_wr.write(0);

    lcd_db.output();
        
    lcd_db = dat >> 8;

    lcd_cs.write(0);
    lcd_cs.write(1);

    lcd_db = dat;

    lcd_cs.write(0);
    lcd_cs.write(1);

    lcd_wr.write(1);

    lcd_db.input();
}

void TSLCDOutDat2(unsigned char dath,unsigned char datl) //write data to LCD
{
    lcd_rs.write(1);

    lcd_rd.write(1);
    lcd_wr.write(0);

    lcd_db.output();
        
    lcd_db = dath;    

    lcd_cs.write(0);
    lcd_cs.write(1);

    lcd_db = datl;

    lcd_cs.write(0);
    lcd_cs.write(1);

    lcd_wr.write(1);

    lcd_db.input();
}

void TSLCDOutIns(unsigned short ins) //write instruction to LCD
{
    lcd_rs.write(0);

    lcd_rd.write(1);
    lcd_wr.write(0);

    lcd_db.output();
        
    lcd_db = ins >> 8;

    lcd_cs.write(0);
    lcd_cs.write(1);

    lcd_db = ins;

    lcd_cs.write(0);
    lcd_cs.write(1);

    lcd_wr.write(1);

    lcd_db.input();
}

unsigned short TSLCDInDat(void) //read data from LCD
{
    unsigned short dat = 0;

    lcd_db.input();

    lcd_rs.write(1);

    lcd_wr.write(1);
    lcd_rd.write(0);

    lcd_cs.write(0);
    lcd_cs.write(0);
    dat = lcd_db;
    lcd_cs.write(1);

    dat <<= 8;

    lcd_cs.write(0);
    lcd_cs.write(0);
    dat |= lcd_db;
    lcd_cs.write(1);

    lcd_rd.write(1);

    return (dat);
}

unsigned short TSLCDInIns(void) //read data from LCD
{
    unsigned short ins = 0;

    lcd_db.input();

    lcd_rs.write(0);

    lcd_wr.write(1);
    lcd_rd.write(0);

    lcd_cs.write(0);
    lcd_cs.write(0);
    ins = lcd_db;
    lcd_cs.write(1);

    ins <<= 8;

    lcd_cs.write(0);
    lcd_cs.write(0);
    ins |= lcd_db;
    lcd_cs.write(1);

    lcd_rd.write(1);

    return (ins);
}

void TSLCDRst(void) //pulse reset signal to LCD
{
    lcd_rst.write(0);
    wait_ms(50);
    lcd_rst.write(1);
}

void TSLCDInit(void) //initial LCD
{
    wait_ms(100);

    TSLCDOutIns(0x00E5);
    TSLCDOutDat(0x8000);                     //set the internal vcore voltage
    TSLCDOutIns(TS_INS_START_OSC);
    TSLCDOutDat(0x0001);                     //start oscillator
    wait_ms(50);

    TSLCDOutIns(TS_INS_DRIV_OUT_CTRL);
    TSLCDOutDat(0x0100);                     //set SS, SM
    TSLCDOutIns(TS_INS_DRIV_WAV_CTRL);
    TSLCDOutDat(0x0700);                     //set 1 line inversion
    
    TSLCDOutIns(TS_INS_ENTRY_MOD);
    TSLCDOutDat(TS_VAL_ENTRY_MOD);            //set GRAM write direction, BGR=0

    TSLCDOutIns(TS_INS_RESIZE_CTRL);
    TSLCDOutDat(0x0000);                     //no resizing

    TSLCDOutIns(TS_INS_DISP_CTRL2);
    TSLCDOutDat(0x0202);                     //front & back porch periods = 2
    TSLCDOutIns(TS_INS_DISP_CTRL3);
    TSLCDOutDat(0x0000);                     
    TSLCDOutIns(TS_INS_DISP_CTRL4);
    TSLCDOutDat(0x0000);                     
    TSLCDOutIns(TS_INS_RGB_DISP_IF_CTRL1);
    TSLCDOutDat(0x0000);                     //select system interface                
    TSLCDOutIns(TS_INS_FRM_MARKER_POS);
    TSLCDOutDat(0x0000);                     
    TSLCDOutIns(TS_INS_RGB_DISP_IF_CTRL2);
    TSLCDOutDat(0x0000);                    
    
    TSLCDOutIns(TS_INS_POW_CTRL1);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_POW_CTRL2);
    TSLCDOutDat(0x0000);                     
    TSLCDOutIns(TS_INS_POW_CTRL3);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_POW_CTRL4);
    TSLCDOutDat(0x0000);                     
    wait_ms(200);

    TSLCDOutIns(TS_INS_POW_CTRL1);
    TSLCDOutDat(0x17B0);
    TSLCDOutIns(TS_INS_POW_CTRL2);
    TSLCDOutDat(0x0137);                     
    wait_ms(50);

    TSLCDOutIns(TS_INS_POW_CTRL3);
    TSLCDOutDat(0x013C);
    wait_ms(50);

    TSLCDOutIns(TS_INS_POW_CTRL4);
    TSLCDOutDat(0x1400);
    TSLCDOutIns(TS_INS_POW_CTRL7);
    TSLCDOutDat(0x0007);
    wait_ms(50);

    TSLCDOutIns(TS_INS_GRAM_HOR_AD);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_GRAM_VER_AD);
    TSLCDOutDat(0x0000);

    TSLCDOutIns(TS_INS_GAMMA_CTRL1);
    TSLCDOutDat(0x0007);
    TSLCDOutIns(TS_INS_GAMMA_CTRL2);
    TSLCDOutDat(0x0504);
    TSLCDOutIns(TS_INS_GAMMA_CTRL3);
    TSLCDOutDat(0x0703);
    TSLCDOutIns(TS_INS_GAMMA_CTRL4);
    TSLCDOutDat(0x0002);
    TSLCDOutIns(TS_INS_GAMMA_CTRL5);
    TSLCDOutDat(0x0707);
    TSLCDOutIns(TS_INS_GAMMA_CTRL6);
    TSLCDOutDat(0x0406);
    TSLCDOutIns(TS_INS_GAMMA_CTRL7);
    TSLCDOutDat(0x0006);
    TSLCDOutIns(TS_INS_GAMMA_CTRL8);
    TSLCDOutDat(0x0404);
    TSLCDOutIns(TS_INS_GAMMA_CTRL9);
    TSLCDOutDat(0x0700);
    TSLCDOutIns(TS_INS_GAMMA_CTRL10);
    TSLCDOutDat(0x0A08);

    TSLCDOutIns(TS_INS_HOR_START_AD);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_HOR_END_AD);
    TSLCDOutDat(0x00EF);
    TSLCDOutIns(TS_INS_VER_START_AD);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_VER_END_AD);
    TSLCDOutDat(0x013F);
    TSLCDOutIns(TS_INS_GATE_SCAN_CTRL1);
    TSLCDOutDat(0x2700);
    TSLCDOutIns(TS_INS_GATE_SCAN_CTRL2);
    TSLCDOutDat(0x0001);
    TSLCDOutIns(TS_INS_GATE_SCAN_CTRL3);
    TSLCDOutDat(0x0000);

    TSLCDOutIns(TS_INS_PART_IMG1_DISP_POS);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_PART_IMG1_START_AD);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_PART_IMG1_END_AD);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_PART_IMG2_DISP_POS);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_PART_IMG2_START_AD);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_PART_IMG2_END_AD);
    TSLCDOutDat(0x0000);

    TSLCDOutIns(TS_INS_PANEL_IF_CTRL1);
    TSLCDOutDat(0x0010);
    TSLCDOutIns(TS_INS_PANEL_IF_CTRL2);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_PANEL_IF_CTRL3);
    TSLCDOutDat(0x0003);
    TSLCDOutIns(TS_INS_PANEL_IF_CTRL4);
    TSLCDOutDat(0x0110);
    TSLCDOutIns(TS_INS_PANEL_IF_CTRL5);
    TSLCDOutDat(0x0000);
    TSLCDOutIns(TS_INS_PANEL_IF_CTRL6);
    TSLCDOutDat(0x0000);

    TSLCDOutIns(TS_INS_DISP_CTRL1);
    TSLCDOutDat(0x0173);
}

void TSLCDShowPic2(ts_pos_t sx,ts_pos_t ex,ts_pos_t sy,ts_pos_t ey,const unsigned short *pic,ts_mode_t mode)
//show picture from code memory with specific size
{
    unsigned long k = 0;
    unsigned short color;
    unsigned int x,y;
    unsigned int i,j;
    if (sx < ts_margin_xl)
        sx = ts_margin_xl;
    if (ex > ts_margin_xr)
        ex = ts_margin_xr;
    if (sy < ts_margin_yu)
        sy = ts_margin_yu;
    if (ey > ts_margin_yl)             
        ey = ts_margin_yl;

    TSLCDOutIns(TS_INS_START_ADX);
    TSLCDOutDat(sx);
    TSLCDOutIns(TS_INS_END_ADX);
    TSLCDOutDat(ex);
    TSLCDOutIns(TS_INS_GRAM_ADX);
    TSLCDOutDat(sx);
    x = ex - sx + 1;

#ifndef TS_ORN_PORTRAIT
    sy = TS_SIZE_Y - 1 - sy;     // mirror start y address
    ey = TS_SIZE_Y - 1 - ey;     // mirror end y address
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(ey);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(sy);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy);//fix from bug of v1_00
    y = sy - ey + 1;
#else
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(sy);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(ey);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy);
    y = ey - sy + 1;
#endif

    TSLCDOutIns(TS_INS_RW_GRAM);

    if (mode == TS_MODE_FULL)
    {
        for (j=0; j<y; j++)
            for (i=0; i<x; i++)
            {
                TSLCDOutDat(*(&pic[k]));
                k++;
            }
    }
    else
    if (mode == TS_MODE_NORMAL)
    {
        for (j=0; j<y; j++)
            for (i=0; i<x; i++)
            {
                if (*(&pic[k]) == TS_COL_WHITE)
                {
                    color = TSLCDInDat();         // ignore invalid data
                    color = TSLCDInDat();
                    TSLCDOutDat(color);
                }
                else
                {
                    TSLCDOutDat(*(&pic[k]));
                }
                k++;
            }
    }
}

void TSLCDFillRect(ts_pos_t sx,ts_pos_t ex,ts_pos_t sy,ts_pos_t ey,unsigned short color,ts_mode_t mode) //draw a rectangular
{
    unsigned int x,y;
    unsigned int i,j;
    if (sx < ts_margin_xl)
        sx = ts_margin_xl;
    if (ex > ts_margin_xr)
        ex = ts_margin_xr;
    if (sy < ts_margin_yu)
        sy = ts_margin_yu;
    if (ey > ts_margin_yl)             
        ey = ts_margin_yl;

    TSLCDOutIns(TS_INS_START_ADX);
    TSLCDOutDat(sx);
    TSLCDOutIns(TS_INS_END_ADX);
    TSLCDOutDat(ex);
    TSLCDOutIns(TS_INS_GRAM_ADX);
    TSLCDOutDat(sx);
    x = ex - sx + 1;

#ifndef TS_ORN_PORTRAIT
    sy = TS_SIZE_Y - 1 - sy;     // mirror start y address
    ey = TS_SIZE_Y - 1 - ey;     // mirror end y address
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(ey);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(sy);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy);//fix from bug of v1_00
    y = sy - ey + 1;
#else
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(sy);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(ey);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy);
    y = ey - sy + 1;
#endif

    TSLCDOutIns(TS_INS_RW_GRAM);

    if ((mode == TS_MODE_NORMAL) || (mode == TS_MODE_FULL))
    {
        for (j=0; j<y; j++)
            for (i=0; i<x; i++)
            {
                TSLCDOutDat(color);
            }
    }
    else
    if (mode == TS_MODE_INVERSE)
    {
        for (j=0; j<y; j++)
            for (i=0; i<x; i++)
            {
                color = TSLCDInDat();         // ignore invalid data
                color = TSLCDInDat();
                TSLCDOutDat(~color);
            }
    }
}

void TSLCDFillCirc(ts_pos_t cx,ts_pos_t cy,ts_pos_t rad,unsigned short color, ts_mode_t mode) //draw a circle
{
#ifndef TS_ORN_PORTRAIT
    int sy_buf,ey_buf;
#endif
    int sx,sy,ex,ey;
    int i,j;
    unsigned short color_buf;
    unsigned short rad2 = rad*rad;
    sx = cx - rad;
    ex = cx + rad;
    sy = cy - rad;
    ey = cy + rad;

    if (sx < ts_margin_xl)
        sx = ts_margin_xl;
    if (ex > ts_margin_xr)
        ex = ts_margin_xr;
    if (sy < ts_margin_yu)
        sy = ts_margin_yu;
    if (ey > ts_margin_yl)             
        ey = ts_margin_yl;

    TSLCDOutIns(TS_INS_START_ADX);
    TSLCDOutDat(sx);
    TSLCDOutIns(TS_INS_END_ADX);
    TSLCDOutDat(ex);
    TSLCDOutIns(TS_INS_GRAM_ADX);
    TSLCDOutDat(sx);
//    x = ex - sx + 1;

#ifndef TS_ORN_PORTRAIT
    sy_buf = TS_SIZE_Y - 1 - sy;     // mirror start y address
    ey_buf = TS_SIZE_Y - 1 - ey;     // mirror end y address
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(ey_buf);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(sy_buf);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy_buf);//fix from bug of v1_00
//    y = sy_buf - ey_buf + 1;
#else
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(sy);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(ey);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy);
//    y = ey - sy + 1;
#endif

    TSLCDOutIns(TS_INS_RW_GRAM);

    if (mode == TS_MODE_NORMAL)
    {
        for (j=sy-cy; j<=ey-cy; j++)
            for (i=sx-cx; i<=ex-cx; i++)
            {
                if ((i)*(i) + (j)*(j) < rad2)
                {
                    TSLCDOutDat(color);
                }
                else
                {
                    color_buf = TSLCDInDat();         // ignore invalid data
                    color_buf = TSLCDInDat();
                    TSLCDOutDat(color_buf);
                }
            }
    }
    else
    if (mode == TS_MODE_INVERSE)
    {
        for (j=sy-cy; j<=ey-cy; j++)
            for (i=sx-cx; i<=ex-cx; i++)
            {
                if ((i)*(i) + (j)*(j) < rad2)
                {
                    color_buf = TSLCDInDat();         // ignore invalid data
                    color_buf = TSLCDInDat();
                    TSLCDOutDat(~color_buf);
                }
                else
                {
                    color_buf = TSLCDInDat();         // ignore invalid data
                    color_buf = TSLCDInDat();
                    TSLCDOutDat(color_buf);
                }
            }
    }
    else
    if (mode == TS_MODE_FULL)
    {
        for (j=sy-cy; j<=ey-cy; j++)
            for (i=sx-cx; i<=ex-cx; i++)
            {
                if ((i)*(i) + (j)*(j) < rad2)
                {
                    TSLCDOutDat(color);
                }
                else
                {
                    TSLCDOutDat(back_color);
                }
            }
    }
}

void TSLCDSetMargins(ts_pos_t xl,ts_pos_t xr,ts_pos_t yu,ts_pos_t yl) //set margins for FillRect,FillCirc
{
    ts_margin_xl = xl;
    ts_margin_xr = xr;
    ts_margin_yu = yu;
    ts_margin_yl = yl;
}

void TSLCDSetMarginsDefault(void) //Reset margins to default value
{
    ts_margin_xl = 0;
    ts_margin_xr = TS_SIZE_X - 1;
    ts_margin_yu = 0;
    ts_margin_yl = TS_SIZE_Y - 1;
}

void buf_store(unsigned char charactor)
{
    unsigned char i,j;
    int char_p = charactor*FONT_SIZE;

    for (i=0; i<FONT_BIT_WIDTH; i++)
        for (j=0; j<FONT_BYTE_HIGHT; j++)
        {
            char_buf[i][j] = *(&font[char_p]);
            char_p++;
        }
}

unsigned char buf_read(unsigned char column,unsigned char row)
{
    unsigned char read_pixel;

    if (row < 8)
    {
        read_pixel = (char_buf[column][0] >> (7-row)) & 0x01;
    }
    else
    {
        row = row - 8;
        read_pixel = (char_buf[column][1] >> (7-row)) & 0x01;
    }
    return (read_pixel);
}

void TSLCDCharDisp(char charactor,ts_pos_t sx,ts_pos_t sy,ts_mode_t mode) //low level function to print a character on LCD
{
    unsigned int x,y;
    unsigned char i,j;
    ts_pos_t ex,ey;
    unsigned short c;

    ex = sx + FONT_WIDTH - 1;
    ey = sy + FONT_HEIGHT - 1;

    buf_store(charactor - 0x20);

    TSLCDOutIns(TS_INS_START_ADX);
    TSLCDOutDat(sx);
    TSLCDOutIns(TS_INS_END_ADX);
    TSLCDOutDat(ex);
    TSLCDOutIns(TS_INS_GRAM_ADX);
    TSLCDOutDat(sx);
    x = ex - sx + 1;

#ifndef TS_ORN_PORTRAIT
    sy = TS_SIZE_Y - 1 - sy;     // mirror start y address
    ey = TS_SIZE_Y - 1 - ey;     // mirror end y address
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(ey);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(sy);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy);//fix from bug of v1_00
    y = sy - ey + 1;
#else
    TSLCDOutIns(TS_INS_START_ADY);
    TSLCDOutDat(sy);
    TSLCDOutIns(TS_INS_END_ADY);
    TSLCDOutDat(ey);
    TSLCDOutIns(TS_INS_GRAM_ADY);
    TSLCDOutDat(sy);
    y = ey - sy + 1;
#endif

    TSLCDOutIns(TS_INS_RW_GRAM);

    if (mode == TS_MODE_NORMAL)
    {
        for (j=y; j>0; j--)
            for (i=0; i<x; i++)
            {
                if (buf_read(i,j))
                {
                    TSLCDOutDat(font_color);
                }
                else
                {
                    c = TSLCDInDat();         // ignore invalid data
                    c = TSLCDInDat();
                    TSLCDOutDat(c);
                }
            }
    }
    else
    if (mode == TS_MODE_INVERSE)
    {
        for (j=0; j<y; j++)
            for (i=0; i<x; i++)
            {
                c = TSLCDInDat();             // ignore invalid data
                c = TSLCDInDat();
                if (buf_read(i,j))
                {
                    TSLCDOutDat(~c);
                }
                else
                {
                    TSLCDOutDat(c);
                }
            }
    }
    else
    if (mode == TS_MODE_FULL)
    {
        for (j=0; j<y; j++)
            for (i=0; i<x; i++)
            {
                if (buf_read(i,j))
                {
                    TSLCDOutDat(font_color);
                }
                else
                {
                    TSLCDOutDat(back_color);
                }
            }
    }
}

void TSLCDSetOffset(ts_pos_t x,ts_pos_t y) //set LCD offset for character display
{
    offsetx = x;
    offsety = y;
}

void TSLCDPrintStr(unsigned char line,unsigned char column,char *str,ts_mode_t mode) //print string on LCD
{
    int i = 0;
    ts_pos_t posx,posy;
    posx = offsetx + column*FONT_WIDTH;
    posy = offsety + line*FONT_HEIGHT;

    while(str[i])
    {
        TSLCDCharDisp(str[i],posx,posy,mode);
        posx += FONT_WIDTH;
        i++;
    }
}

void TSLCDPrintTxt(unsigned char line,unsigned char column,const char *txt,ts_mode_t mode) //print text from code memory
{
    int i = 0;
    ts_pos_t posx,posy;
    posx = offsetx + column*FONT_WIDTH;
    posy = offsety + line*FONT_HEIGHT;

    while((&txt[i]))
    {
        TSLCDCharDisp(*(&txt[i]),posx,posy,mode);
        posx += FONT_WIDTH;
        i++;
    }
}

void TSLCDPrintCh(unsigned char line,unsigned char column,char c,ts_mode_t mode) //print a character on LCD
{
    ts_pos_t posx,posy;
    posx = offsetx + column*FONT_WIDTH;
    posy = offsety + line*FONT_HEIGHT;

    TSLCDCharDisp(c,posx,posy,mode);
}
/////

int main() {

	//setSystemFrequency(3, 1, 16, 1);
       
    // Do QVGA Setup Stuff
    TSLCDRst();
    TSLCDInit();
    
    wait_ms(100);
    lcd_bl.write(1);
    
    TSLCDFillRect(0,TS_SIZE_X-1,0,TS_SIZE_Y-1,TS_COL_BLUE,TS_MODE_NORMAL);
    TSLCDFillRect(0,TS_SIZE_X-1,0,70,TS_COL_WHITE,TS_MODE_NORMAL);
    TSLCDSetFontColor(TS_COL_BLUE);
    TSLCDPrintStr(2,6,"Testing ELT240320TP with AVR",TS_MODE_NORMAL);
    TSLCDFillRect(20,80,90,130,TS_COL_BLACK,TS_MODE_NORMAL);
    TSLCDFillRect(30,90,100,140,TS_COL_YELLOW,TS_MODE_NORMAL);
    TSLCDFillRect(20,80,160,200,TS_COL_BLACK,TS_MODE_NORMAL);
    TSLCDFillRect(30,90,170,210,TS_COL_RED,TS_MODE_NORMAL);
    TSLCDFillRect(195,205,71,TS_SIZE_Y-1,TS_COL_WHITE,TS_MODE_NORMAL);
    TSLCDFillCirc(200,155,60,TS_COL_WHITE,TS_MODE_NORMAL);
    TSLCDFillCirc(200,155,50,TS_COL_BLUE,TS_MODE_NORMAL);
    TSLCDFillCirc(200,155,40,TS_COL_BLACK,TS_MODE_NORMAL);
    TSLCDFillCirc(200,155,30,TS_COL_RED,TS_MODE_NORMAL);

    TSLCDFillRect(5, 15, 5, 15, TS_COL_GREEN,TS_MODE_NORMAL);
    
    while(1){}
}