#include "../i2c/ad0x0_i2c1.h"
#include "../i2c/ad0x0_i2c2.h"
#include "../i2c/ad0x0_i2c_ext.h"
#include <string.h>
#include "ssd1327.h"
//unsigned char vbuf[100];
#ifdef USE_BACKBUFFER
#include "math.h"
#endif
    
#include "font7x10.h"
void OLED_Init(oled_desc_s *p_desc);
void OLED_test(oled_desc_s *p_desc);
void OLED_test2(oled_desc_s *p_desc);
void OLED_test3(oled_desc_s *p_desc);
	void OLED_Printf(oled_desc_s *p_desc,unsigned char* buf, unsigned char size);  /* печатает строку с размерами - 0 самый мелкий 2 - увеличиный - 3 самый большой(только для цифр) */
	void OLED_DrawImage(unsigned char num_image);  /* выводит картинку из флеш */
	void OLED_Bat(unsigned char y,unsigned char x, unsigned char z);  /* рисует батарейку по координатам z - от 0 до 10 делений */
	void OLED_Mode(oled_desc_s *p_desc, char set_mode);  /* 1 - inverted / 0 - normal */
	void OLED_Sleep(oled_desc_s *p_desc, char set);  /* 1 - on sleep / 0 - off sleep */
	void OLED_Command(oled_desc_s *p_desc, unsigned char ControByte, unsigned char DataByte);
	void OLED_Command2(oled_desc_s *p_desc, unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2);
	void OLED_Command3(oled_desc_s *p_desc, unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2,unsigned char DataByte3);
	void OLED_Command_buf(oled_desc_s *p_desc, unsigned char *pbuf,uint8_t count);
	void OLED_PrintAt(oled_desc_s *p_desc, unsigned char x, unsigned char y,unsigned char* buf, unsigned char size);
	void OLED_Char2x(oled_desc_s *p_desc, unsigned int c);  /* auaia neiaiea ia 2 no?iee*/
	void OLED_GotoXY(oled_desc_s *p_desc, unsigned char x, unsigned char y);
	void OLED_GotoXYWH(oled_desc_s *p_desc, unsigned char x, unsigned char y,unsigned char w, unsigned char h);
	void OLED_Clear(oled_desc_s *p_desc);  /* i?enoea anaai aenieay */
#ifdef USE_BACKBUFFER
	void	buf_render(oled_desc_s *p_desc);
	portFORCE_INLINE float	ad0x0_getdist(int16_t x,int16_t y);
//	portFORCE_INLINE uint16_t	buf_getpixelcol(uint8_t x,uint8_t y);
	portFORCE_INLINE void	buf_setpixelcol(uint8_t x,uint8_t y,uint8_t c);
	uint16_t  sqrxi32(uint32_t y);
	void OLED_Command_framebuf(oled_desc_s *p_desc);
#endif
//	void OLED_BigNum(unsigned char num);  /* auaia aieuoeo oeo? */
	void OLED_Char(oled_desc_s *p_desc, unsigned int c);  /* auaia neiaiea */

oled_desc_s *p_desc_i2c1=NULL,*p_desc_i2c2=NULL;//жопой чую, что пожалею, что сунул все в один си файл
#ifdef USE_BACKBUFFER
	__packed struct i2c_cheat_frame_buf_s{
		uint8_t i2c_0x40;
		uint8_t framebuf[8192];		
	};
	struct i2c_cheat_frame_buf_s framebuf;
	struct demopoint_s{
		int16_t x,y;
		int8_t dir_x,dir_y;
		float	weight;
	};
#endif
//
void command_i2c1(unsigned char ControByte, unsigned char DataByte){
	OLED_Command(p_desc_i2c1,ControByte,DataByte);
}
void command_i2c2(unsigned char ControByte, unsigned char DataByte){
	OLED_Command(p_desc_i2c2,ControByte,DataByte);
}

void command_buf_i2c1(unsigned char *pbuf,uint8_t count){
	OLED_Command_buf(p_desc_i2c1,pbuf,count);
}
void command_buf_i2c2(unsigned char *pbuf,uint8_t count){
	OLED_Command_buf(p_desc_i2c2,pbuf,count);
}

void command2_i2c1(unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2){
	OLED_Command2(p_desc_i2c1,ControByte,DataByte1,DataByte2);
}
void command2_i2c2(unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2){
	OLED_Command2(p_desc_i2c2,ControByte,DataByte1,DataByte2);
}

void command3_i2c1(unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2,unsigned char DataByte3){
	OLED_Command3(p_desc_i2c1,ControByte,DataByte1,DataByte2,DataByte3);
}
void command3_i2c2(unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2,unsigned char DataByte3){
	OLED_Command3(p_desc_i2c2,ControByte,DataByte1,DataByte2,DataByte3);
}

void OLED_PrintAt_i2c1(unsigned char x, unsigned char y,unsigned char* buf, unsigned char size){
	OLED_PrintAt(p_desc_i2c1,x,y,buf,size);
}
void OLED_PrintAt_i2c2(unsigned char x, unsigned char y,unsigned char* buf, unsigned char size){
	OLED_PrintAt(p_desc_i2c2,x,y,buf,size);
}

void 		OLED_test_i2c1(void){
	OLED_test(p_desc_i2c1);
}
void 		OLED_test_i2c2(void){
	OLED_test(p_desc_i2c2);
}

void 		OLED_test2_i2c1(void){
	OLED_test2(p_desc_i2c1);
}
void 		OLED_test2_i2c2(void){
	OLED_test2(p_desc_i2c2);
}

void 		OLED_test3_i2c1(void){
	OLED_test3(p_desc_i2c1);
}
void 		OLED_test3_i2c2(void){
	OLED_test3(p_desc_i2c2);
}

void 		OLED_clear_i2c1(void){
	OLED_Clear(p_desc_i2c1);
}
void 		OLED_clear_i2c2(void){
	OLED_Clear(p_desc_i2c2);
}

void	OLED_Init2(I2C_TypeDef * _I2C_BASE, oled_desc_s *p_desc){
	memset(p_desc,0,sizeof(oled_desc_s));
	p_desc->font_w=8;
	p_desc->font_h=10;
	framebuf.i2c_0x40=0x40;
	memset(framebuf.framebuf,0,8192);
	if(_I2C_BASE == (I2C_TypeDef *)I2C1_BASE){
			p_desc_i2c1=p_desc;
			p_desc->ad0x0_i2c_get_ext_async2=ad0x0_i2c1_get_ext_async2;
			p_desc->ad0x0_i2c_push=ad0x0_i2c1_push;
			p_desc->command=command_i2c1;
			p_desc->command_buf=command_buf_i2c1;
			p_desc->command2=command2_i2c1;
			p_desc->command3=command3_i2c1;
			p_desc->print_at=OLED_PrintAt_i2c1;
			p_desc->OLED_test=OLED_test_i2c1;
			p_desc->OLED_test2=OLED_test2_i2c1;
			p_desc->OLED_test3=OLED_test3_i2c1;
			p_desc->clear=OLED_clear_i2c1;
			OLED_Init(p_desc_i2c1);
	}else
	if(_I2C_BASE == (I2C_TypeDef *)I2C2_BASE){		
			p_desc_i2c2=p_desc;
			p_desc->ad0x0_i2c_get_ext_async2=ad0x0_i2c2_get_ext_async2;
			p_desc->ad0x0_i2c_push=ad0x0_i2c2_push;
			p_desc->command=command_i2c2;
			p_desc->command_buf=command_buf_i2c2;
			p_desc->command2=command2_i2c2;
			p_desc->command3=command3_i2c2;
			p_desc->print_at=OLED_PrintAt_i2c2;
			p_desc->OLED_test=OLED_test_i2c2;
			p_desc->OLED_test2=OLED_test2_i2c2;
			p_desc->OLED_test3=OLED_test3_i2c2;
			p_desc->clear=OLED_clear_i2c2;
			OLED_Init(p_desc_i2c2);
	}else{		
			ad0x0_err();
		}
}

/* ---------------------------------------------------------
*  OLED_init
*  ---------------------------------------------------------
*/
void OLED_Init(oled_desc_s *p_desc)
{
	//OLED_Sleep(0);
  p_desc->command(COMAND, SSD1327_DISPLAYOFF);
  //HAL_Delay(10);
	ad0x0_i2c1_push(ADQ_DELAY_MS,100);	
  p_desc->command(COMAND, SSD1327_DISPLAYON);
  ad0x0_i2c1_push(ADQ_DELAY_MS,100);	
	p_desc->command2(COMAND, SSD1327_SEGREMAP, 0x53);//ad0x0
	
	/*p_desc->command(COMAND, SSD1306_SEGREMAP | 0x0);//ssd1327
	p_desc->command(COMAND, 0x51);
  p_desc->command(COMAND, SSD1306_SETCONTRAST);
	p_desc->command(COMAND, 0x7f);
        
	p_desc->command(COMAND, SSD1306_SETDISPLAYCLOCKDIV);
	p_desc->command(COMAND, 0x80);
	p_desc->command(COMAND, SSD1306_SETMULTIPLEX);
	p_desc->command(COMAND, 0x3F);
	p_desc->command(COMAND, SSD1306_SETDISPLAYOFFSET);
	p_desc->command(COMAND, 0x00);
	p_desc->command(COMAND, SSD1306_SETSTARTLINE | 0x00);
	p_desc->command(COMAND, SSD1306_CHARGEPUMP);
	p_desc->command(COMAND, 0x14);
	p_desc->command(COMAND, SSD1306_MEMORYMODE);
	p_desc->command(COMAND, 0x00);
	//p_desc->command(COMAND, SSD1306_SEGREMAP | 0x1);//ad0x0
  p_desc->command(COMAND, SSD1306_SEGREMAP | 0x0);//ad0x0
	//p_desc->command(COMAND, SSD1306_COMSCANDEC);//ad0x0
  p_desc->command(COMAND, SSD1306_COMSCANINC);
	p_desc->command(COMAND, SSD1306_SETCOMPINS);
	//p_desc->command(COMAND, 0x12);
  p_desc->command(COMAND, 0x2);//ad0x0
	p_desc->command(COMAND, SSD1306_SETCONTRAST);
	p_desc->command(COMAND, 0xCF);
	p_desc->command(COMAND, SSD1306_SETPRECHARGE);
	p_desc->command(COMAND, 0xF1);
	p_desc->command(COMAND, SSD1306_SETVCOMDETECT);
	//p_desc->command(COMAND, 0x40);
  p_desc->command(COMAND, 0x20);
	p_desc->command(COMAND, SSD1306_DISPLAYALLON_RESUME);
	OLED_Mode(p_desc, 0);
	OLED_Sleep(p_desc, 1);*/
p_desc->command(COMAND, SSD1306_DISPLAYALLON_RESUME);
	//OLED_Clear(p_desc);
	OLED_GotoXY(p_desc, 0,0);
}

/*128x128, это x[0-127],y[0-127]*/
void OLED_GotoXY(oled_desc_s *p_desc, unsigned char x, unsigned char y)
{
	if(y & 0x1)ad0x0_err();
	p_desc->char_x = x;
	p_desc->char_y = y;
	//столбцов 128, однако в одном байте "два столбца" (2 ниббла (16 градаций серого))
	p_desc->command3(COMAND, SSD1327_COLUMNADDR,x>>1,63); //0x21//0x15 0-0x63
  //p_desc->command(COMAND, x>>1);
  //p_desc->command(COMAND, 63);//==
	p_desc->command3(COMAND, SSD1327_PAGEADDR,y,0x7F);//0x22  //0x75
	//p_desc->command(COMAND, y);  
	//p_desc->command(COMAND, 0x7F);  
}

void OLED_GotoXYWH(oled_desc_s *p_desc, unsigned char x, unsigned char y,unsigned char w, unsigned char h){
  if(y & 0x1)ad0x0_err();
	p_desc->char_x = x;
	p_desc->char_y = y;
	p_desc->command3(COMAND, SSD1327_COLUMNADDR,x>>1,(x>>1)+(w>>1)-1);  
	//p_desc->command(COMAND, x>>1);  //начало строки
	//p_desc->command(COMAND, (x>>1)+(w>>1));  //конец (после переполенения след.столб.+начало строки)
	p_desc->command3(COMAND, SSD1327_PAGEADDR,y,0x7f);
	//p_desc->command(COMAND, y);//столбец старт
	//p_desc->command(COMAND, 0x7f);//
}

void OLED_Char2x(oled_desc_s *p_desc, unsigned int c)
{
	ad0x0_err();
	/*unsigned char x = 0;
  uint8_t suka=4,v,nv;
  //вызов OLED_GotoXYWH(x,y,10,2) должен установить окно размером 10пикс х 2строки
  //типа было 10110111 => 1100111100111111
	for (x=0; x<5; x++)
	{
          v=OLED_Buffer[c*5+x];
          nv=0;
          suka=4;
          while(suka--){
            if(v & 0x8){nv|=0x3;}
            if(suka)nv<<=2;
            v<<=1;
          }
        p_desc->command(DATA, nv);
        p_desc->command(DATA, nv);
        //p_desc->command(DATA, 0x0);
	}	
        for (x=0; x<5; x++)
	{
          v=OLED_Buffer[c*5+x];
          nv=0;
          suka=4;
          while(suka--){
            if(v & 0x80){nv|=0x3;}
            if(suka)nv<<=2;
            v<<=1;
          }
        p_desc->command(DATA, nv);
        p_desc->command(DATA, nv);
        //p_desc->command(DATA, 0x0);
	}	
	
    p_desc->OLED_X += 11;
	if(p_desc->OLED_X>SSD1306_LCDWIDTH)
	{
		p_desc->OLED_X = SSD1306_DEFAULT_SPACE;
	}
    */
    
}

/* ---------------------------------------------------------
*  OLED_Clear
*  ---------------------------------------------------------
*/
void OLED_Clear(oled_desc_s *p_desc)
{
	uint16_t i;
	uint8_t buf16[16];
	OLED_GotoXYWH(p_desc, 0,0,128,128);
	for (i=0; i<16; i++)buf16[i]=0x0;
	for (i=0; i<8192/16; i++)
	{
		p_desc->command_buf(buf16,16);
	}
	
	p_desc->char_x = 0;
	p_desc->char_y = 0;
}


/* ---------------------------------------------------------
*  OLED_Char
предполагается, что окно в памяти ссд1327 для отрисовки чара
уже выставлено, мы просто заполняем его данными
*  ---------------------------------------------------------
*/
void OLED_Char(oled_desc_s *p_desc, unsigned int c)
{
	unsigned char y = 10;
  uint8_t *pfchar=(uint8_t *)Font7x10_u8 + (c-0x20)*10;
	uint8_t pbuf[16],pbuf_i=0;
	/*for (x=0; x<5; x++)
	{
        p_desc->command(DATA, OLED_Buffer[c*5+x]);
	}	*/
	while(y)
	{
		uint8_t b=*pfchar;
		uint8_t shc=4;
		uint8_t r;		
		while(shc){			
			if(b & 0x40)r=0xF0;else r=0;
			if(b & 0x80)r|=0x0F;else r&=0xF0;
			b<<=2;
			shc--;
			//p_desc->command(DATA, r);//по одному байту слать это тормозно, нах надо, будем кусками по 16 байт
			pbuf[pbuf_i++]=r;
			if(pbuf_i>15){
				p_desc->command_buf(pbuf,16);
				pbuf_i=0;
			}
		}
		pfchar++;
		y--;
	}
	if(pbuf_i>0)p_desc->command_buf(pbuf,pbuf_i);//последний кусок
}
void OLED_PrintAt(oled_desc_s *p_desc, unsigned char x, unsigned char y,unsigned char* buf, unsigned char size){
  
  while (*buf!=0)
	{
		if(size==0)
		{
			if((x+p_desc->font_w)>SSD1306_LCDWIDTH){
				x=0;
				y+=10;
				if(SSD1306_LCDHEIGHT < (y+p_desc->font_h))y=0;
			}
			
			OLED_GotoXYWH(p_desc, x,y,8,10);
			OLED_Char(p_desc, *buf++);
			x+=8;			
		}
	}
	p_desc->char_x = x;
}

void OLED_Command(oled_desc_s *p_desc, unsigned char ControByte, unsigned char DataByte){
  /*if(ad0x0_i2c_isovrflow()){
    asm("nop");
  }else
  {
    asm("nop");
  }*/
  //ad0x0_i2c1_push(0x1,0);
  //ad0x0_i2c1_push(0x3,SSD1306_I2C_ADDRESS & 0xFE);
  /*ad0x0_i2c1_push(ADQ_START_ADDR,SSD1306_I2C_ADDRESS & 0xFE);//v2 start+addr
  ad0x0_i2c1_push(ADQ_TX,ControByte);
  ad0x0_i2c1_push(ADQ_TX_STOP,DataByte);
	
	ad0x0_i2c2_push(ADQ_START_ADDR,SSD1306_I2C_ADDRESS & 0xFE);//v2 start+addr
  ad0x0_i2c2_push(ADQ_TX,ControByte);
  ad0x0_i2c2_push(ADQ_TX_STOP,DataByte);*/
	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	p_desc->ad0x0_i2c_get_ext_async2(&h_ext,(void**)&p_ext_t);
	
	p_ext_t->addr_w=SSD1306_I2C_ADDRESS;
	p_ext_t->user_data0=ControByte | (uint32_t)(DataByte<<8);
	p_ext_t->pbuf_count=2;
	p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->user_data0;//TX!!!!!!!
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
  p_desc->ad0x0_i2c_push(ADQ_HAL_ADDR_TX_BUF_STOP_EXT,h_ext);	
}

void OLED_Command2(oled_desc_s *p_desc, unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2){
	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	p_desc->ad0x0_i2c_get_ext_async2(&h_ext,(void**)&p_ext_t);
	
	p_ext_t->addr_w=SSD1306_I2C_ADDRESS;
	p_ext_t->user_data0=ControByte | (uint32_t)(DataByte1<<8) | (uint32_t)(DataByte2<<16);
	p_ext_t->pbuf_count=3;
	p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->user_data0;//TX!!!!!!!
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
  p_desc->ad0x0_i2c_push(ADQ_HAL_ADDR_TX_BUF_STOP_EXT,h_ext);	
}

void OLED_Command3(oled_desc_s *p_desc, unsigned char ControByte, unsigned char DataByte1,unsigned char DataByte2,unsigned char DataByte3){
	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	p_desc->ad0x0_i2c_get_ext_async2(&h_ext,(void**)&p_ext_t);
	
	p_ext_t->addr_w=SSD1306_I2C_ADDRESS;
	p_ext_t->user_data0=ControByte | (uint32_t)(DataByte1<<8) | (uint32_t)(DataByte2<<16)| (uint32_t)(DataByte3<<24);
	p_ext_t->pbuf_count=4;
	p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->user_data0;//TX!!!!!!!
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
  p_desc->ad0x0_i2c_push(ADQ_HAL_ADDR_TX_BUF_STOP_EXT,h_ext);	
}

void OLED_Command_buf(oled_desc_s *p_desc, unsigned char *pbuf,uint8_t count){
	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	p_desc->ad0x0_i2c_get_ext_async2(&h_ext,(void**)&p_ext_t);
	/*void (*_p_post_cb_func)(uint8_t _sf_id,ad0x0_i2c_desc_s *pdspi,void *p_i2c_ext_t);
	uint32_t user_data0,user_data1;<----------------------- 8 байт
	void *user_pointer0,*user_pointer1;//<----------------------- 8 байт
	uint8_t reference_count;//ячейка не освобождается >0!!!!!!!!!!!!!*/
	if(count>16)ad0x0_err();//два раза по 8 байт
	p_ext_t->addr_w=SSD1306_I2C_ADDRESS;
	//p_ext_t->user_data0=ControByte | (uint32_t)(DataByte1<<8) | (uint32_t)(DataByte2<<16)| (uint32_t)(DataByte3<<24);
	p_ext_t->pbuf_count=count+1;
	//p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->user_data0;//TX!!!!!!!
	p_ext_t->OLED_i2c_databyte=DATA;
	memcpy((uint8_t*)&p_ext_t->user_data0,pbuf,count);
	p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->OLED_i2c_databyte;
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
  p_desc->ad0x0_i2c_push(ADQ_HAL_ADDR_TX_BUF_STOP_EXT,h_ext);	
}

void OLED_Command_framebuf(oled_desc_s *p_desc){
	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	p_desc->ad0x0_i2c_get_ext_async2(&h_ext,(void**)&p_ext_t);
	p_ext_t->addr_w=SSD1306_I2C_ADDRESS;
	p_ext_t->pbuf_count=8193;
	p_ext_t->pbuf_tx=(uint8_t*)&framebuf.i2c_0x40;
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
  p_desc->ad0x0_i2c_push(ADQ_HAL_ADDR_TX_BUF_STOP_EXT,h_ext);		
}
/* ---------------------------------------------------------
*  OLED_test
*  ---------------------------------------------------------
*/
void OLED_test(oled_desc_s *p_desc)
{

//OLED_Clear(p_desc);
OLED_PrintAt(p_desc, 0,0,(unsigned char*)"Шрифт размер = 0",0);   
osDelay(1500);
OLED_PrintAt(p_desc, 0,10,(unsigned char*)"Шрифт s = 1",0);  
osDelay(1500);
OLED_PrintAt(p_desc, 0,20,(unsigned char*)"Шрифт s = 2",0);  
osDelay(1500);
OLED_Clear(p_desc);
}

void OLED_test2(oled_desc_s *p_desc)
{
	uint16_t i;
	//unsigned short x = 0;	unsigned short y = 0;
	uint32_t stFill,stText,msFill,msText;
	uint8_t buf16[16];
	unsigned char strf[32],strt[32],str64[64];
	stFill=xTaskGetTickCount();
	OLED_GotoXYWH(p_desc, 0,0,128,128);
	for (i=0; i<16; i++)buf16[i]=0x33;
	for (i=0; i<8192/16; i++)
	{
		p_desc->command_buf(buf16,16);
	}
	msFill=xTaskGetTickCount()-stFill;
	//for (i=0; i<(SSD1306_LCDWIDTH*SSD1306_LCDHEIGHT/8); i++) /* (SSD1306_LCDWIDTH*SSD1306_LCDHEIGHT/8) */
	/*osDelay(1000);
	OLED_GotoXYWH(p_desc, 10,0,8,10);
	OLED_Char(p_desc, '#');
	OLED_GotoXYWH(p_desc, 20,0,8,10);
	OLED_Char(p_desc, '1');
	OLED_GotoXYWH(p_desc, 30,0,8,10);
	OLED_Char(p_desc, '2');
	OLED_GotoXYWH(p_desc, 40,0,8,10);
	OLED_Char(p_desc, '3');*/
	
	stText=xTaskGetTickCount();
	for(i=0;i<16;i++){
	if(i<12){
		OLED_GotoXYWH(p_desc, 0,i*10,8,10);
		OLED_Char(p_desc, '0'+(i<10?i:i-10));
	}
	if(i<12){
		OLED_GotoXYWH(p_desc, i*8,i*10,8,10);
		OLED_Char(p_desc, '0'+(i<10?i:i-10));
	}
	OLED_GotoXYWH(p_desc, i*8,0,8,10);
	OLED_Char(p_desc, '0'+(i<10?i:i-10));
	}
	msText=xTaskGetTickCount()-stText;
	sprintf((char*)strf,"fill %dms",msFill);	
	p_desc->print_at(30,10,strf,0);
	sprintf((char*)strt,"text %dms",msText);	
	p_desc->print_at(30,20,strt,0);
	sprintf((char*)str64,"text %dms,text %dms,text %dms,text %dms,text %dms,text %dms",msText,msText,msText,msText,msText,msText);	
	//p_desc->print_at(40,30,str64,0);
	/*for (i=0; i<128; i++) 
	{
		p_desc->command(DATA, i);
//		osDelay(300);
	}*/

}

unsigned char * i16_2s(int16_t value, unsigned char *buffer)
{
  memset(buffer,' ',6);
   buffer += 7;
   char m=(value<0)?'-':' ';
   if(value<0)value=-value;
// 11 байт достаточно для десятичного представления 32-х байтного числа
// и  завершающего нуля
   *--buffer = 0;
   do
   {
      *--buffer = value % 10 + '0';
      value /= 10;
   }
   while (value != 0);
   *--buffer=m;
   return buffer;
}
unsigned char * i32_2s(int32_t value, unsigned char *buffer)
{
  memset(buffer,' ',8);
   buffer += 7;
   char m=(value<0)?'-':' ';
   if(value<0)value=-value;
// 11 байт достаточно для десятичного представления 32-х байтного числа
// и  завершающего нуля
   *--buffer = 0;
   do
   {
      *--buffer = value % 10 + '0';
      value /= 10;
   }
   while (value != 0);
   *--buffer=m;
   return buffer;
}

#ifdef USE_BACKBUFFER

void	buf_render(oled_desc_s *p_desc){
OLED_Command_framebuf(p_desc);
/*	uint16_t i;
	uint8_t buf16[16];
	OLED_GotoXYWH(p_desc, 0,0,128,BUF_HEIGHT);
	for (i=0; i<128*BUF_HEIGHT/2; i+=16)
	{
		memcpy(buf16,framebuf.framebuf+i,16);
		p_desc->command_buf(buf16,16);
	}		*/
}

float	ad0x0_getdist(int16_t x,int16_t y){
//	return sqrt(x*x+y*y);
	return sqrxi32(x*x+y*y);
}

void	buf_setpixelcol(uint8_t x,uint8_t y,uint8_t c){
uint8_t *addr=(framebuf.framebuf + (uint16_t)(y<<6) + (x>>1));
	if( x & 0x1){
		*addr&=0x0F;
		*addr|=c<<4;
	}else{
		*addr&=0xF0;
		*addr|=c;		
	}
}

uint16_t  sqrxi32(uint32_t y)
{
	
	if(y==1) return 1;
	uint32_t xh =  y>0x10000ul?0x10000ul:y;
	uint32_t xl  =  0;
	uint32_t xc ;
	for(int k=0;k<16;k++)
	{
		xc =  (xh+xl)>>1ul;
		if(xc*xc-xc>=y)
		{
			xh = xc;
		}
		else
		{
			xl = xc;
		}
	}
	return (xh+xl)>>1ul;
}

/*uint16_t	buf_getpixelcol(uint8_t x,uint8_t y){
	uint8_t c=*(framebuf + (uint16_t)(y<<6) + (x>>1));
	return ( x & 0x1 ? c>>4 : c & 0xF0);
}*/

void OLED_test3(oled_desc_s *p_desc)
{
	int16_t x,y;
	uint8_t i;
	static struct demopoint_s points[3]={{40,60,2,2,6.0},{100,20,-2,-2,2.5},{60,100,1,-1,1.0}};
	float	rescol;
	static float	dist;
	uint8_t *addr=framebuf.framebuf,col;
	
	
	for (y=0; y<BUF_HEIGHT; y++)
	for (x=0; x<128; x+=2)//в одном байте 2 пикселя... нахер нам бить его на куски, будем херачить как 64х128
	{
		rescol=0x0;
		for(i=0;i<2;i++){//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			dist=ad0x0_getdist(points[i].x-x,points[i].y-y);
			rescol+=((128-dist)/dist * points[i].weight)*0.333;// /3
		}
		col=round(rescol);
		col=(col>15?15:col);
		if(!col)col=1;
		//buf_setpixelcol(x,y,(rescol>15?15:round(rescol)));
		//*(framebuf + (uint16_t)(y<<6) + (x>>1))=(col<<4)|col;
		*addr++=(col<<4)|col;
	}
	for(i=0;i<3;i++){
			buf_setpixelcol(points[i].x,points[i].y,0);
			points[i].x+=points[i].dir_x;
			points[i].y+=points[i].dir_y;
			if(points[i].x<1){points[i].dir_x=-points[i].dir_x;points[i].x=1;}
			if(points[i].x>127){points[i].dir_x=-points[i].dir_x;points[i].x=127;}
		
			if(points[i].y<1){points[i].dir_y=-points[i].dir_y;points[i].y=1;}
			if(points[i].y>BUF_HEIGHT-1){points[i].dir_y=-points[i].dir_y;points[i].y=BUF_HEIGHT-1;}
			rescol=0xF;
			buf_setpixelcol(points[i].x,points[i].y,(rescol>15?15:rescol));
	}
//	buf_render(p_desc);	
}
#endif
