uint8_t search_sensors(uint8_t currBus)
{
//    uint8_t i;
    uint8_t id[OW_ROMCODE_SIZE];
    uint8_t diff;

    debug_puts( "?" );
    //	debug_puts( "Scan for DS18X20\n" );

    for( diff = OW_SEARCH_FIRST;
         (diff != OW_LAST_DEVICE) && (nTempSensors < N_TEMPERATURE_IN) ; )
    {
        DS18X20_find_sensor( &diff, &id[0] );

        if( diff == OW_PRESENCE_ERR )
        {
            //			debug_puts( "No Sensor\n" );
            debug_puts( "-" );
            break;
        }

        if( diff == OW_DATA_ERR )
        {
            REPORT_ERROR(ERR_FLAG_1WIRE_SCAN_FAULT);
            //			debug_puts( "Bus Error\n" );
            debug_puts( "B" );
            //ow_error_cnt++;
            //ow_bus_error_cnt[currBus];
            count_1w_bus_error( currBus );
            break;
        }

        //		debug_puts( "OK! got some!\n" );
        debug_puts( "+" );


#if OW_SERIAL_ID
        if(id[0] == DS2401_ID)
        {
            ow_copy_rom( serialNumber, id );
//            for( i=0; i < OW_ROMCODE_SIZE; i++ )
//                serialNumber[i]=id[i];
        }
        else
#endif
        {
#if N_TEMPERATURE_IN > 0
            ow_copy_rom( gTempSensorIDs[nTempSensors], id );
//            for( i=0; i < OW_ROMCODE_SIZE; i++)
//                gTempSensorIDs[nTempSensors][i]=id[i];

            gTempSensorBus[nTempSensors] = currBus;
#endif
            nTempSensors++;
        }

    }

    return nTempSensors;
}

unsigned char Ax_Send( volatile unsigned char *message_global, int str_length) {
	int i;
	debug_puts("\n message length... \t");
	debug_putdec(str_length);
	debug_puts("\n/////////\n");
	for(i=0;i<str_length;i++){
		FM_buffer[FM_buffer_num][i]=message_global[i];
		debug_puthex(message_global[i]);
		debug_puts(",");
	}
	debug_puts("\n---------\n");
	for(i=0;i<str_length;i++){
		debug_putc(message_global[i]);
	}
	debug_puts("\n/////////\n");
	FM_buffer_length[FM_buffer_num]=str_length;
	FM_buffer_num++;
	if(FM_buffer_num>FM_BUFFER_MAX-1){
		FM_buffer_num=0;
	}
	if(FM_mode==0||FM_mode==3){
		FM_mode=1;	//モード0ならtonさせる
	}
	return 1;
}

static void
do_logic(game_state *state)
{
    switch_state_data *data = state->data->switch_data;

    if (data->mouse_down_last_step && state->game->mouse_down == false)
    {
        /* clicked and released; create a circle */
        cpFloat radius = 15;
        cpFloat mass = 10;
        cpFloat moment = cpMomentForCircle(mass, 0, radius, cpvzero);

        cpBody *ball = cpSpaceAddBody(data->space, cpBodyNew(mass, moment));
        cpBodySetPos(ball, state->game->mouse_pos);

        cpShape *ball_shape = cpSpaceAddShape(data->space,
                cpCircleShapeNew(ball, radius, cpvzero));

        cpShapeSetElasticity(ball_shape, 0.7);
        cpShapeSetFriction(ball_shape, 0.7);
        cpShapeSetLayers(ball_shape, L_PLAYER);
    }

    cpSpaceStep(data->space, TARGET_SEC_PER_FRAME);
    data->mouse_down_last_step = state->game->mouse_down;

    if (ent_switch_get_state(data->sw))
        debug_puts("switch is on");
    else
        debug_puts("switch is off");
}

int
uinput_sendkey (int fd, uint16_t key, int32_t value)
{
	struct input_event ev[2];

	memset(&ev, 0, sizeof ev);
	ev[0].type = EV_KEY;
	ev[0].code = key;
	ev[0].value = value;
	ev[1].type = EV_SYN;
	ev[1].code = SYN_REPORT;

	debug_puts("Sending EV_KEY event to uinput");
	DEBUG_FN(debug_event(&(ev[0])));
	uinput_write(fd, &ev[0], sizeof ev[0]);
	debug_puts("Sending EV_SYN event to uinput");
	DEBUG_FN(debug_event(&(ev[1])));
	uinput_write(fd, &ev[1], sizeof ev[1]);
	return 0;

	/*
	 * Unworking code, sends the EV_KEY and EV_SYN together. On RPi, only
	 * every eigth EV_SYN is received.
	 */
	uinput_write(fd, ev, sizeof ev);
	return 0;
}

void clearreg(void)
{
    uint32_t pinaddr, ind = 0;
    debug_puts("Start clearreg\n\r");
    Xil_Out32(XPSS_SYS_CTRL_BASEADDR + 0x8, SLCR_UNLOCK_MAGIC); //slcr_unlock
    /* remap DDR to zero, FILTERSTART */
    Xil_Out32(XPSS_SCU_BASEADDR + 0x40, 0);                     //filter_start
    /* Device config APB, unlock the PCAP */
    Xil_Out32(XPSS_DEV_CFG_APB_BASEADDR + 0x34, 0x757BDF0D);    //unlock
    Xil_Out32(XPSS_DEV_CFG_APB_BASEADDR + 0x28, 0xFFFFFFFF);    //rom_shadow
    /* OCM_CFG, Mask out the ROM, map ram into upper addresses */
    Xil_Out32(XPSS_SYS_CTRL_BASEADDR + 0x910, 0x1F);            //ocm_cfg
    /* FPGA_RST_CTRL, clear resets on AXI fabric ports */
    Xil_Out32(XPSS_SYS_CTRL_BASEADDR + 0x240, 0);             //fpga_rst_ctrl
    /* TZ_DDR_RAM, Set DDR trust zone non-secure */
    Xil_Out32(XPSS_SYS_CTRL_BASEADDR + 0x430, 0xFFFFFFFF);      //trust_zone
    /* Set urgent bits with register */
    Xil_Out32(XPSS_SYS_CTRL_BASEADDR + 0x61c, 0);             //ddr_urgent_sel
    /* Urgent write, ports S2/S3 */
    Xil_Out32(XPSS_SYS_CTRL_BASEADDR + 0x600, 0xC);             //ddr_urgent
    while ((pinaddr = sdio1_pindef[ind].pinaddr)) {
#ifndef EMIO_SDIO1
        /* release pin set tri-state */
        Xil_Out32(pinaddr, (Xil_In32(pinaddr) & ~XSLCR_MIO_LMASK) | XSLCR_MIO_PIN_XX_TRI_ENABLE);
        /* assign pin to this peripheral */
        Xil_Out32(pinaddr, sdio1_pindef[ind].enable);
#endif
        ind++;
    }
    Xil_Out32(XPSS_SYS_CTRL_BASEADDR + 0x4, SLCR_LOCK_MAGIC);   //slcr_lock
    debug_puts("Jump to linux\n\r");
    _binary_imagefiles_zImage_start(0, XILINX_EP107, 0x1000000 /* address of devicetree data */);
}

UBYTE sd_cmd58()
{
	BYTE r;
	sd_wait_ready();

	spi_send_byte(0x7a);
	spi_send_byte(0x00);
	spi_send_byte(0x00);
	spi_send_byte(0x00);
	spi_send_byte(0x00);
	spi_send_byte(0xff);

	r = sd_wait_r1();
	spi_delay(); // if &0xc0==0xc0 => SDHC
	spi_delay();
	spi_delay();
	spi_delay();

#ifdef DEBUG_SD
	debug_puts("cmd58:");
	debug_print_byte(r);
	debug_puts("\n");
#endif
	return r;
}

UBYTE sd_cmd8()
{
	BYTE r;
	sd_wait_ready();

	spi_send_byte(0x48);
	spi_send_byte(0x00);
	spi_send_byte(0x00);
	spi_send_byte(0x01);
	spi_send_byte(0xaa);
	spi_send_byte(0x87);

	r = sd_wait_r1();
	spi_delay();
	spi_delay();
	spi_delay();
	spi_delay();
	spi_delay();

#ifdef DEBUG_SD
	debug_puts("cmd8:");
	debug_print_byte(r);
	debug_puts("\n");
#endif
	return r;
}

/*
 * Задача выдачи статистики на консоль.
 */
void console (void *arg)
{
    /* Разрешаем FPU для текущей задачи.
     * Округление к ближайшему целому, исключения отключены. */
    task_fpu_control (task_current, FCSR_ROUND_N, FCSR_ROUND | FCSR_ENABLES);

    for (;;) {
        debug_puts ("\33[H");
        debug_puts ("Testing FPU.\n\n");

        debug_printf ("     Duration: %02u:%02u:%02u \n", timer_hour, timer_min, timer_sec);
        debug_printf ("CPU frequency: %u MHz\n", KHZ / 1000);
        debug_printf ("   Reg.CSCON3: %08x  \n", MC_CSCON3);
#ifdef ENABLE_ICACHE
        debug_printf ("  Instr.cache: enabled  \n\n");
#else
        debug_printf ("  Instr.cache: disabled  \n\n");
#endif
        debug_printf ("---------Value-----------------Loops-------Errors----\n");
        debug_printf ("     e:  %-20.15f  %-10u  %u \n", e, count_e_loops, count_e_errors);
        debug_printf ("Machin:  %-20.15f  %-10u  %u \n", pi_machin, count_machin_loops, count_machin_errors);
        debug_printf (" Gauss:  %-20.15f  %-10u  %u \n", pi_gauss, count_gauss_loops, count_gauss_errors);
        debug_printf (" Brent:  %-20.15f  %-10u  %u \n", pi_brent, count_brent_loops, count_brent_errors);

        /* Выдача каждые полсекунды. */
        mutex_wait (&timer_half_sec);
    }
}

int main(void) {
  hal_system_clock();

  // FIXME: this is platform-specific, add start-wait hooks to HAL
  log_init();

  debug_puts("Initializing stuff!\r\n");
  hal_system_init();

  hal_system_info();

  debug_puts("Starting Timers!\r\n");
  hal_system_start();

  debug_puts("Entering FreeEMS!\r\n");

  setup_emulation();

  main_freeems();

  while(1) {
    debug_puts("STUCK IN MEANINGLESS INFINITE LOOP! (main.c 43)");
  }

  debug_puts("Everything is over :-(");
  return 0;
}

void task_tcl (void *arg)
{
	unsigned char *cmd;
	unsigned char result, got_partial, quit_flag;
	Tcl_Interp *interp;
	Tcl_CmdBuf buffer;

	configure_ram ();
	mem_init (&pool, (size_t) RAM_START, (size_t) RAM_END);
again:
	debug_printf ("\nEmbedded TCL\n\n");

	interp = Tcl_CreateInterp (&pool);
	Tcl_CreateCommand (interp, (unsigned char*) "loop", loop_cmd, 0, 0);
	Tcl_CreateCommand (interp, (unsigned char*) "echo", echo_cmd, 0, 0);

	buffer = Tcl_CreateCmdBuf (&pool);
	got_partial = 0;
	quit_flag = 0;
	while (! quit_flag) {
/*		clearerr (stdin);*/
		if (! got_partial) {
			debug_puts ("% ");
		}
		if (! debug_gets (line, sizeof (line))) {
			if (! got_partial)
				break;

			line[0] = 0;
		}
		cmd = Tcl_AssembleCmd (buffer, line);
		if (! cmd) {
			got_partial = 1;
			continue;
		}

		got_partial = 0;
		result = Tcl_Eval (interp, cmd, 0, 0);

		if (result != TCL_OK) {
			debug_puts ("Error");

			if (result != TCL_ERROR)
				debug_printf (" %d", result);

			if (*interp->result != 0)
				debug_printf (": %s", interp->result);

			debug_putchar (0, '\n');
			continue;
		}

		if (*interp->result != 0)
			debug_printf ("%s\n", interp->result);
	}

	Tcl_DeleteInterp (interp);
	Tcl_DeleteCmdBuf (buffer);
	goto again;
}

int sd_load_sector(UBYTE* target, DWORD sector)
{
	DWORD address;
	BYTE r;
	BYTE timeout;

	address = sector<<9;

#ifdef DEBUG_SD
	debug_puts("loading address ");
	debug_print_dword(address);
	debug_puts("\n");
#endif

	sd_wait_ready();
	// read block
	spi_assert_cs();
	spi_send_byte(0x51);		// CMD17
	spi_send_byte((address>>24)&0xff);
	spi_send_byte((address>>16)&0xff);
	spi_send_byte((address>>8)&0xff);
	spi_send_byte(address&0xff);
	spi_send_byte(0xff);

	r = sd_wait_r1();
#ifdef DEBUG_SD
	debug_puts("cmd17:");
	debug_print_byte(r);
	debug_puts("\n");
#endif
	if ((r&0x80)!=0) {
		spi_deassert_cs();
		return FALSE;
	}

	// wait for 0xfe (start of block)
	timeout = 0xff;
	while (spi_receive_byte()!=0xfe) {
		if (timeout==0) {
			spi_deassert_cs();
			return FALSE;
		}
		timeout = timeout-1;
	}

	// read block
	load_data(target);

	// skip crc
	spi_delay();
	spi_delay();

	// shutdown
	spi_delay();
	spi_deassert_cs();
	return TRUE;
}

void debug_command(char* str) {
  unsigned int i;
  
  debug_puts("Program got command: ");
  debug_puts(str);
  debug_puts("\r\n");

  if (str[0] == 's') {
  }
}

/*************************************************
            ITU0結果表示関数
*************************************************/
unsigned char itu0_display(void)
{
	int time = itu0_cnt*2;
	debug_puts("time = ");
	debug_putdec(time);
	debug_puts("us\n");
	itu0_cnt = 0;					//カウントクリア
	
	return 1;
}

void mem_print_free_list (mem_pool_t *m)
{
	mheader_t *h;

	debug_puts ("free list:");
	mutex_lock (&m->lock);
	for (h=(mheader_t*)m->free_list; h; h=NEXT(h)) {
		debug_printf (" %p-%p", h, (char*)h + h->size - 1);
        }
	mutex_unlock (&m->lock);
	debug_puts ("\n");
}

int main()
{
  debug_puts(
    "\n"
    "This program tests the simulator magic exit port.\n"
    "There should be no more messages after this one.\n"
  );

  sim_exit(4);

  debug_puts( "ERROR - Simulation did not exit?\n" );

  return 0;
}

/*
 * Задача выдачи статистики на консоль.
 */
void console (void *arg)
{
	for (;;) {
		mutex_wait (&timer_lock);
		debug_puts ("\33[H");
		debug_puts ("Measuring interrupt latency.\n\n");

		debug_printf ("CPU frequency: %u MHz\n", KHZ / 1000);
		debug_printf ("\n");
		debug_printf ("   Interrupts: %u  \n\n", nirqs);

		print_rational (" Latency, min: ", latency_min * 1000, KHZ);
		print_rational ("          max: ", latency_max * 1000, KHZ);
	}
}

/*************************************************
        ミッションCPU強制終了
*************************************************/
unsigned char PronounceMissionAbort(void)
{
	debug_puts("!!!!!!-- Pronounce Shutodown to Mission --!!!!!!.\n");
	debug_puts("r-g-c-asd \r\n");
	
	sci2_puts("c-m-c-asd \r\n");
	
	// タイムアウト処理用タイマ(ITU2)カウント開始
	ITU.TISRA.BIT.IMFA2 = 0;	// 割り込みステータスフラグクリア
	ITU.TISRA.BIT.IMIEA2 = 1;	// タイマ割り込み許可
	ITU.TSTR.BIT.STR2 = 1;		// カウント開始
	
//	Ax_Send("r-g-c-asd \r\n",12);
	
	return 1;
}

void uos_init (void)
{
    /* Задаём количество wait states для внешней flash-памяти на nCS3. */
    //MC_CSCON3 = MC_CSCON_WS (CS3_WAIT_STATES);

    /* Стираем экран. */
    debug_puts ("\33[H\33[2J");

    /* Используем интервальный таймер в масштабе 1:1. */
    MC_ITCSR = 0;
    MC_ITSCALE = 0;
    MC_ITPERIOD = KHZ * MSEC_PER_TICK - 1;
    MC_ITCSR = MC_ITCSR_EN;
    mutex_attach_irq (&timer_lock, TIMER_IRQ, timer_handler, 0);

    /* Запускаем задачу выдачи статистики на консоль. */
    task_create (console, 0, "console", 10, task_console, sizeof (task_console));

    /* Запускаем задачи вычислений.
     * По таймеру будем менять их приоритет, чтобы добиться
     * большого количества переключений контекста. */
    t1 = task_create (test_fpu1, 0, "fpu", 2, task_fpu1, sizeof (task_fpu1));
    t2 = task_create (test_fpu2, 0, "fpu", 1, task_fpu2, sizeof (task_fpu2));
    t3 = task_create (test_fpu3, 0, "fpu", 1, task_fpu3, sizeof (task_fpu3));
    t4 = task_create (test_fpu4, 0, "fpu", 1, task_fpu4, sizeof (task_fpu4));
}

void replay(void *arg) {
	void *slave = zmq_socket(arg, ZMQ_REQ);
	/*
	if (done % 2) {
		zmq_connect (slave, "tcp://localhost:5562");
	}
	else {
		zmq_connect (slave, "tcp://localhost:5563");
	}*/
	zmq_connect (slave, "tcp://localhost:5562");

	char *string;
	char buf[1024];
	sprintf(buf, "%d", rand());
	s_send(slave, buf);
	string = s_recv(slave);
	if (strcmp(string , buf) != 0) {
		debug_puts("ERROR");
	}
	free(string);
    pthread_mutex_lock(&lock);
    done++;
    pthread_mutex_unlock(&lock);
	zmq_close(slave);
}

/*
 * Read a newline-terminated string from stream.
 */
unsigned char *
debug_gets (unsigned char *buf, int len)
{
	int c;
	unsigned char *s;

	s = buf;
        while (--len > 0) {
		c = debug_getchar ();
		if (c == '\b') {
			if (s > buf) {
				--s;
				debug_puts ("\b \b");
			}
			continue;
		}
		if (c == '\r')
			c = '\n';
		debug_putchar (0, c);
		*s++ = c;
		if (c == '\n')
			break;
	}
	*s = '\0';
	return buf;
}