static ssize_t do_read(int fd, void *buf, size_t count)
{
  ssize_t s = 0;
  unsigned char *p = buf;
  for ( ; s < count; ) {
    int ch = next_char();
    ++s;                        // Count the character.
    switch (ch) {
      // Some simple line handling.
    case 0x7F:
    case '\b':
      // Simple backspace handling.
      --s;
      if (s) {
        send_char('\b');
        send_char(' ');
        send_char('\b');
        --p;
        --s;
      }
      break;
    case '\n':
    case '\r':
      // Make sure we send both a CR and LF.
      send_char(ch == '\r' ? '\n' : '\r');
      *p = '\n';                // and send a newline back.
      return s;                 // This read is done.
    default:
      send_char(ch);            // Echo input.
      *p++ = ch;                // and send it back.
      break;
    }
  }
  return s;
}

int fputc(int ch, FILE *f) {
 
 if ( ch == 0x0a	)
	send_char(0x0d);

	return (send_char(ch));
}

/////////////////////////////// string to display 5x8 ///////////////////////////////
void ssd0323::zeichen_small_1x(const uint8_t *font,unsigned char z, int spalte, int zeile, int back, int color, unsigned char offset){
	unsigned char a ,b ,c;
	unsigned int stelle;

	if((z<0x20)||(z>0x7f))z=0x20;
	stelle = 8*(z-0x20);

	send_command(0x15);
	send_command(3*spalte+floor(offset/2)); // die sind 6 breit => immer 2 auf einmal
	send_command((3*spalte)+2+floor(offset/2)); // (6*spalte)+5 => 0,1,2,3,4 das sind 5
	send_command(0x75);
	send_command(8*zeile);
	send_command((8*zeile)+7);

	for(c=0;c<8;c++){
		a = pgm_read_byte(&(font[stelle]));

		b=(back&0x0f)+((back&0x0f)<<4); /* default sende 2 pixel mit backlight */
		if(a&0x80)b = (b & 0x0f) + ((color&0x0f)<<4); /*alibi funktion für XX[1]X XXXX ... kommt nicht vor daher leerstelle */
		if(a&0x40)b = (b & 0xf0) + (color&0x0f); /*wenn XXX[1] XXXX dann sende 4 bit backlight/char, 4 bit char*/
		send_char(b);

		b=(back&0x0f)+((back&0x0f)<<4); /* default sende 2 pixel mit backlight */
		if(a&0x20)b = (b & 0x0f) + ((color&0x0f)<<4); /* wenn XXXX [1]XXX dann sende 4 bit char und dann 4 bit backlight */
		if(a&0x10)b = (b & 0xf0) + (color&0x0f); /* wenn XXXX X[1]XX dann sende 4 bit backlight und 4 bit back/char */
		send_char(b);

		b=(back&0x0f)+((back&0x0f)<<4);
		if(a&0x08)b = (b & 0x0f) + ((color&0x0f)<<4);/* wenn XXXX XX[1]X dann sende 4 bit char und dann 4 bit backlight */
		if(a&0x04)b = (b & 0xf0) + (color&0x0f);/* wenn XXXX XXX[1] dann sende 4 bit char und dann 4 bit back/char */
		send_char(b);

		stelle++;
	}
}

//implementation of __io_putchar for printf, to be used with STM's UART line
int __io_putchar(int c)
{
	if (c=='\n')
		send_char('\r'); //carriage return to remove starirs effect
	send_char(c);
	return c;
}

void show_tbl(unsigned char *tbl)
{
	for (int i=0; i<256;i++)
	{
		send_byte(tbl[i]);
		send_char(' ');
	}
	send_char('\n');
}

void UART4_IRQHandler()
{
	if (USART_GetITStatus(UART4, USART_IT_RXNE) != RESET)
	{
		char gps_letter = USART_ReceiveData(UART4);
		if(gps_letter != 0) cbAdd(&GPS_BUFFER, gps_letter);
		send_char(gps_letter);	//potrzebne do debugowania
		send_char('a');
	}
	USART_ClearITPendingBit(UART4, USART_IT_RXNE);
}

static void key_handle_unmod(kbd_event_t *ev)
{
	switch (ev->key) {
	case KC_ENTER:
		send_char('\n');
		break;
	default:
		if (ev->c >= 32 || ev->c == '\t' || ev->c == '\b') {
			send_char(ev->c);
		}
	}
}

void Example_UART_Print(void)
{

    /* Initialize system */
    SIO_Configuration(UART1);

    SIO_ChInit();

    /* enable SIO1 trx int */
    NVIC_EnableIRQ(INTTX1_IRQn);

    //Send char one by one
    send_char('C');
    send_char('o');
    send_char('o');
    send_char('C');
    send_char('o');
    send_char('x');
    send_char('\r');
    send_char('\n');
    
    //Send chars as string
    send_str("Hello, CooCox\r\n");

    while (1);
}

int	send_str(pid_t pid, const char *str)
{
  int	x;

  x = 0;
  send_pid(pid);
  while (str[x] != '\0')
    {
      if (send_char(pid, str[x]))
	return (1);
      x += 1;
    }
  send_char(pid, 0);
  return (0);
}

void uplink_cmd_handler(void) {
    
    static char cmd[20];
    static uint8_t index = 0;
    
    // No data received, return
    if(buffer_level(UPLINK_USART,RX) < 1) return;
    
    // flush cmd buffer if cmd is out of a valid length
    if(index == MAX_CMD_LENGTH) {
        index = 0;
    }

    // append char to cmd
    recv_char(UPLINK_USART, &cmd[index]);
    
    switch(cmd[index]) {
        case '\r':
            // carriage return received, replace with stringtermination and parse
            send_str(UPLINK_USART, "\r\n");
            cmd[index] = '\0';
            parse_cmd(cmd);
            index = 0;
        break;
        case '\n':
            // do nothing, but avoid index from incrementing
        break;
        case '\b':
            // backspace, remove last received char
            index--;
            send_char(UPLINK_USART, '\b');
        break;
            // char is part of an ESC sequence
        case 0x1B:
        case 0x5B:
            index++;
        break;
            // each other if the last two char was not part of an ESC sequence
        default:
            if(cmd[index - 1] == 0x5B && cmd[index - 2] == 0x1B) {
                    index = index - 2;
            } else {
                send_char(UPLINK_USART, cmd[index]);
                index++;
            }
    }
  
}

void _ttywrch(int ch) {

  send_char(ch);
  


}

void send_char_show(int portnum, char showchar)
{
  char s[80];
  sprintf(s,"%02X ",(unsigned char) showchar);
  print_string_to(s,0);
  send_char(portnum,showchar);
}

static int consdrv_intrproc(struct consreg *cons) {
  unsigned char c;
  char *p;

  if(serial_is_recv_enable(cons->index)) {
    c = serial_recv_byte(cons->index);
    if(c == '\r') c = '\n';

    send_string(cons, &c, 1);

    if(cons->id) {
      if(c != '\n') cons->recv_buf[cons->recv_len++] = c;

      else {
	p = svc_memalloc(CONS_BUF_SIZE);
	memcpy(p, cons->recv_buf, cons->recv_len);
	svc_send(MLBOX_ID_CONSINPUT, cons->recv_len, p);
	cons->recv_len = 0;
      }
    }
  }

  if(serial_is_send_enable(cons->index)) {
    if(!cons->id || !cons->send_len) serial_intr_send_disable(cons->index);
    else send_char(cons);
  }

  return 0;
}

// grafix has to be stored at /gfx/
int speedo_disp::sd2ssd(char filename[10],int frame){
	send_command(0x15);
	send_command(0x00);
	send_command(0x7F);
	send_command(0x75);
	send_command(0x00);
	send_command(0x3F);

	SdFile root;
	SdFile file;
	SdFile subdir;
	root.openRoot(&pSD->volume);

	if(!subdir.open(&root, "gfx", O_READ))    {  return 1; };
	if(!file.open(&subdir, filename, O_READ)) {  return 2; };
	unsigned long frame_seeker=(unsigned long)frame*64*64;
	if(!file.seekSet(frame_seeker))			  {  return 3; }; // ein bild ist 64*64 Byte groß, da wir 64 lines zu je 64*2*4 Bit Breite haben

	uint8_t buf[65];
	//int n;
	//while ((n = file.read(buf, sizeof(byte)*64)) > 0) {
	for (int zeile=0;	(file.read(buf, sizeof(byte)*64)>0)	&& zeile<64;	zeile++ ) {
		for(int j=0;j<64;j++){
			send_char(buf[j]);
		};
	};
	file.close();
	subdir.close();
	root.close();
	return 0;
};

/*
 * Send execute messae. "conn" should at the point right after the message
 * kind was read.
 */
void
process_execute(char *buf, PGconn *conn)
{
	int			len;
	char	   *portal;
	int			maxrows;
	char	   *bufp;

	SKIP_TABS(buf);

	len = sizeof(int);

	portal = buffer_read_string(buf, &bufp);
	buf = bufp;
	len += strlen(portal) + 1;

	SKIP_TABS(buf);

	fprintf(stderr, "FE=> Execute(portal=\"%s\")\n", portal);

	SKIP_TABS(buf);

	maxrows = buffer_read_int(buf, &bufp);

	len += sizeof(int);

	send_char('E', conn);
	send_int(len, conn);
	send_string(portal, conn);
	send_int(maxrows, conn);
}

void GameObjectMessage::send(uint32_t object_id, GameObject *game_object) {
  send_uint32(socket_, object_id);
  send_vector(socket_, game_object->body().position());
  send_char(socket_, game_object->game_object_type());
  send_object_points(socket_, game_object->characteristic_points());
  send_bool(socket_, game_object->alive());
}

static void console_insert_char( char c )
{
    uint32_t i;
    uint32_t len = strlen( console_buffer );

    /* move the end of the line out to make space */
    for ( i = len + 1; i > console_cursor_position; i-- )
    {
        console_buffer[i] = console_buffer[i - 1];
    }

    /* insert the character */
    len++;
    console_buffer[console_cursor_position] = c;

    /* print out the modified part of the ConsoleBuffer */
    send_str( &console_buffer[console_cursor_position] );

    /* move the cursor back to where it's supposed to be */
    console_cursor_position++;
    for ( i = len; i > console_cursor_position; i-- )
    {
        send_char( '\b' );
    }
}

int main(int argc, char* argv[])
{
    char str[512];
    memset(str, 0, 512 );
        
    if ( argc > 1 )
        strncpy( str, argv[1], 511 );
    else
        return -1;
      
    int index = 0;
    int len = strlen(str);
    char* s = &str[0];
    while( *s )
    {
        char c = *s;
        if ( c == '\\' )
        {
            s++;
            c = *s;
            if ( c == 'r')
            {
                send_virtual_key_press(VK_RETURN);
            }
                
        }  
        else
            send_char( c ); 
        s++;
    }
    
    return 0;     
}

void print(char *str)
{
	while(*str != '\0')
	{
		send_char(*str++);
	}
}

//Get the value from some port
void getPort(int portnum){
 
	unsigned char value;

	if(portnum==22) value=PORTA;
	else if(portnum==1) value=PORTB;
	else if(portnum==2) value=PORTC;
	else if(portnum==3) value=PORTD;
	else if(portnum==4) value=PORTE;
	else if(portnum==5) value=PORTF;
	else if(portnum==6) value=PORTG;
	else if(portnum==7) value=PORTH;
	else if(portnum==8) value=PORTJ;
	else if(portnum==9) value=PORTK;
	else if(portnum==10) value=PORTL;
	else if(portnum==11) value=DDRA;
	else if(portnum==12) value=DDRB;
	else if(portnum==13) value=DDRC;
	else if(portnum==14) value=DDRD;
	else if(portnum==15) value=DDRE;
	else if(portnum==16) value=DDRF;
	else if(portnum==17) value=DDRG;
	else if(portnum==18) value=DDRG;
	else if(portnum==19) value=DDRJ;
	else if(portnum==20) value=DDRK;
	else if(portnum==21) value=DDRL;
	send_char(value);
	//_delay_ms(1000);
}