/* This function is called by libgcc.a's division functions when an attempt
 * is made to divide by zero. If this has happened, something has probably
 * gone wrong, so print an error and stop
 */
void raise(void)
{
	console_write(FG_RED "Error: division by zero attempted\n");
	console_write("STOPPED\n");

	while(1);
}

/* Main kernel entry point, called by trampoline */
void start_kernel(start_info_t * start_info)
{
	/* Define hypervisor upcall entry points */
        HYPERVISOR_set_callbacks(
                FLAT_KERNEL_CS, (unsigned long)hypervisor_callback,
                FLAT_KERNEL_CS, (unsigned long)failsafe_callback);
	/* Map the shared info page */
	HYPERVISOR_update_va_mapping((unsigned long) shared_info, 
			__pte(start_info->shared_info),
			UVMF_INVLPG);
	/* Initialise the console */
	console_init(start_info);
	/* Write a message to check that it worked */
	console_write("Hello world!\n\r");
	console_write("Xen magic string: ");
	console_write(start_info->magic);
	console_write("\n\r");

	/* Set up the XenStore driver */
	xenstore_init(start_info);
	/* Test the store */
	xenstore_test();
	/* Flush the console buffer */
	console_flush();
	/* Exit, since we don't know how to do anything else */
}

static void print_atag_serial(struct atag_serial *data)
{
	console_write("  Serial number: 0x");
	console_write(tohex(data->high, 4));
	console_write(tohex(data->low, 4));
	console_write("\n");
}

void strdump(char* str) {
	int j;
	for (j = 0; j < strlen(str); j++) {
		console_write("%d ", str[j]);
	}
	console_write("|%d |%s \n", strlen(str), str);
}	 

int mhemain(char* romName)
{
	clearScreen();
	console_write(romName,COLOR_GREEN);
	console_write("Loading...",COLOR_GREEN);

    handleInputFile(romName);

	InitInput();

	clearScreen();

    sound_system_init();

	gp_startSound();

    ngpc_run();

	gp_stopSound();

	if (autosave)
		flashShutdown();

	return 0;
}

void
console_printbuf_putc(int ch, void * arg)
{
	struct console_printbuf_state * info = (struct console_printbuf_state *)arg;
	info->total += 1;
	if (info->pos < (SERIAL_CONS_BUF_SIZE - 1)) {
		info->str[info->pos] = ch;
		info->pos += 1;
	} else {
		/*
		 * when len(line) > SERIAL_CONS_BUF_SIZE, we truncate the message
		 * if boot-arg 'drain_uart_sync=1' is set, then
		 * drain all the buffer right now and append new ch
		 */
		if (serialmode & SERIALMODE_SYNCDRAIN) {
			info->str[info->pos] = '\0';
			console_write(info->str, info->pos);
			info->pos            = 0;
			info->str[info->pos] = ch;
			info->pos += 1;
		}
	}

	info->str[info->pos] = '\0';
	/* if newline, then try output to console */
	if (ch == '\n' && info->flags & CONS_PB_WRITE_NEWLINE) {
		console_write(info->str, info->pos);
		info->pos            = 0;
		info->str[info->pos] = '\0';
	}
}

static void console_whiteout(int n)
{
    int t;

    for (t = 0; t < n; t++) console_write(" ",1);
    for (t = 0; t < n; t++) console_write("\b",1);
}

int x86_write(int file, char *ptr, int len)
{
	console_write(ptr, len);	// Ignore the file ID - write all on console regardless
	if (ptr[len - 1] == '\n')
		console_write("\r", 1);	// Add carrige return (hack, migh be avoided by tuning istty)
	return len;		/*Wrote it all (say...) */
};

void fizz_buzz(void)
{
	unsigned char i;
	char *out_index = "";
	for(i = 1; i < 100; i++)
	{
		if(i % 3 == 0)
		{
			console_write("Fizz");
		}
		if(i % 5 == 0)
		{
			console_write("Buzz");
		}
		if(i % 6 == 0)
		{
			console_write("Bang");
		}
		if(!(i % 3 == 0) && !(i % 5 == 0) && !(i % 6 == 0))
		{
			toChar(i, out_index);
			console_write(out_index);
		}
		console_write("\n");
	}
}

static int console_command(console_state cs)
{
  int i,o;
  int ntokens;
  char req[MAX_REQ];
  char tokens[MAX_TOK][MAX_TOKLEN];

  struct console_cmd_params_t p;
  memset(&p,0,sizeof(struct console_cmd_params_t));
  p.cs = cs;

  switch (console_read(cs,req,sizeof(req))) 
    {
    case -1:
      console_write(cs,"   cannot read command\n");
      return CON_QUIT;
    case 0:
      return CON_CMD_NONE;
    default:
      // DBG_PRINT("wsim:con:main: read -%s-\n",req);
      break;
    }

  if ((ntokens = get_tokens(req,tokens,console_ui_delim)) == 0)
    {
      return console_cmd_help(&p);
    }
  
  for(i=0; i < cs->cmd_max ; i++)
    {
      if (strcasecmp(cs->commands[i].name,tokens[0]) == 0)
	{
	  if (cs->commands[i].nb_option > (ntokens - 1))
	    {
	      console_write(cs,"  incorrect number of arguments for %s\n",tokens[0]);
	      return CON_CMD_ERROR;
	    }
	  for(o=0; o < ntokens; o++)
	    {
	      p.options[o] = tokens[o+1];
	    }
	  
	  p.nopt = ntokens - 1;
	  return cs->commands[i].fun(&p);
	}
    }

  console_write(cs,"   unknown command %s\n\n",tokens[0]);

  return CON_CMD_ERROR;
}

void print_rbuf()
{   
    char *dbuf = ringbuf;
    unsigned int end1 = 0, end2 = 0;
    
    if (r_st < r_end) {
        console_write(dbuf + r_st, r_end - r_st);
    }
    else { 
        end1 = MAX_RBUF_LEN  - r_st;
        end2 = r_end;
        console_write(dbuf + r_st, end1);
        console_write(dbuf, end2);
    }
}

int serial_find(int com, int timeout, char* ack1, char* ack2) {
	if (!recvlst) {
		recvlst = List_create();
	}
	int i;
	for (i = 0; i <= timeout * 10; i++) {
		if (!serial_read(com, __serial_buf2)) {
			delay(100);
			continue;
		}	
		console_write(__serial_buf2);

		char* x = strtok(__serial_buf2, "\n");
		while (x != 0) {
			if (ack1 && strstr(x, ack1) == x) {
				return 1;	 
			}
			if (ack2 && strstr(x, ack2) == x) {
				return 1;	 
			}
			x = strtok(0, "\n");
		}						
	}
//	console_write("ERROR/TIMEOUT\n");
	return 0;
}

int putchar(int c)
{
	const char ch = c;

	console_write(&ch, 1);
	return 0;
}

void	console_write_line(const char* str)
{
  console_write(str);
  unsigned space_count = WIDTH - ((scr_count/2) % WIDTH);
  for (; space_count; space_count--)
    printchar(' ');
}

void user_conprint_chanlev (CONSOLE * console, char **list, size_t lsize,
			    const char *suffix)
{
  size_t i;
  for (i = 0; i < lsize; i++)
    console_write (console, " %-34s %s\r\n", list[i], suffix);
}

int kprintf(const char *format, ...)
{
	va_list ap;
	va_start(ap, format);
	int size = kvsprintf(buffer, format, ap);
	return console_write(buffer, size);
}

/******************************************************************************
* Function Name    : rni_console_write
* Function Purpose : Write text on remote console
* Input parameters : RN address
*                    text string, max 18 char, longer texts will be terminated 
*                    with NULL
* Return value     : void
******************************************************************************/
void rni_console_write(uint8_t addr,char *text)
{
    msg_buf_t *text_msg;
    size_t len;

    if(RN_OWN_ADDRESS == addr)
    {
        console_write(text);
        return;
    }

    len = strlen(text);

    if(len + 2 > RN_MAX_MESSAGE_DATA)
    {
        len = RN_MAX_MESSAGE_DATA - 2;
    }

    text_msg = msg_buf_alloc(len+2);
    
    msg_put_uint8(text_msg,0,RRC_CONSOLE_WRITE);

    strcpy((char*)&text_msg->data[1],text);
    
    msg_put_uint8(text_msg,len+2,0);

    rn_rccr_send(addr, text_msg);   
}

static void lnp_console_execute_command(char *out_buffer, int buffer_len, 
		int function_id, char *args) {
	switch (function_id) {
		case COMMAND_ID:
			console_id(out_buffer, buffer_len, args);
			break;
		case COMMAND_WRITE:
			console_write(out_buffer, buffer_len, args);
			break;
		case COMMAND_READ:
			console_read(out_buffer, buffer_len, args);
			break;
		case COMMAND_FLUSH:
			console_flush(out_buffer, buffer_len, args);
			break;
		case COMMAND_CONNECTIONS:
			console_connections(out_buffer, buffer_len, args);
			break;
		case COMMAND_HISTORY:
			console_history(out_buffer, buffer_len, args);
			break;
		case COMMAND_CONNECT:
			console_connect(out_buffer, buffer_len, args);
			break;
		case COMMAND_KEYS:
			console_print_keys(out_buffer, buffer_len, args);
			break;
	}
}

void PM_setOSCursorLocation(int x,int y)
{
    if (!cc)
        return;
#ifndef __QNXNTO__
    console_write(cc, -1, 0, NULL, 0, &y, &x, NULL);
#endif
}

/******************************************************************************
* Function Name    : receive_rrc_console_write
* Function Purpose : Write received text on console
* Input parameters : Message buffer with text string in pos 1
* Return value     : void
******************************************************************************/
void receive_rrc_console_write(msg_buf_t *msg_buf)
{
    char text[64];

    sprintf(text,"RN-addr %d: %s \r\n",msg_return_rn_sender(msg_buf),&msg_buf->data[1]);

    console_write(text);
}