/* Segue lateralmente a parede */
void follow_wall() {
	short dist1, dist14, dist3, dist4;

	/* Se o sonar 1 estiver lendo uma distância muito alta vira à esquerda */
	read_sonar(1, &dist1);
	if(dist1 > THRESHOLD ) {
		set_motors_speed(10, 2);
		busy_wait(300000);
		set_motors_speed(0, 0);
	}
	/* Se o sonar 1 estiver lendo uma distância muito baixa vira à direita */
	else if(dist1 < THRESHOLD - ACCEPTABLE_DIFFERENCE) {
		set_motors_speed(2, 10);
		busy_wait(300000);
		set_motors_speed(0, 0);
	}
	/* Está a uma boa distância da parede */
	else {
		set_motors_speed(10, 10);
		busy_wait(300000);
	}
	/* Se houver uma parede à frente rodar até evitar a parede */
	read_sonar(3, &dist3);
	while(dist3 < THRESHOLD) {
		set_motors_speed(0, 10);
		busy_wait(1000);
		set_motors_speed(0, 0);
		read_sonar(3, &dist3);
	}
}

int main(void)
{
	const unsigned int count = 100; // 待ち時間用ループカウント

	// クロックの設定
	 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

	// GPIO の初期化。PD12, PD13, PD14 and PD15　を出力に設定。
	 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13| GPIO_Pin_14| GPIO_Pin_15;
	 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	 GPIO_Init(GPIOD, &GPIO_InitStructure);


	while(1)
    {
		// LED ON
	    GPIO_SetBits(GPIOD, GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15);
		busy_wait(count);

		// LED OFF
	    GPIO_ResetBits(GPIOD, GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15);
		busy_wait(count);
    }
	return 0;
}

static u_char
spic_call2(struct spic_softc *sc, u_char dev, u_char fn)
{
	busy_wait(sc);
	write_port2(sc, dev);
	busy_wait(sc);
	write_port1(sc, fn);
	return read_port1(sc);
}

void* t1_function(GPIO* output)
{
	for (int i = 0; i < 1000; i++)
	{
		onOff(*output, ON);
		busy_wait(1000000);
		onOff(*output, OFF);
		busy_wait(3000000);
	}
}

void _start() {
	vm_map(0x10000000, (0xAFFFF - 0xA0000), 0xA0000);

	busy_wait(0x5FFFFFF);
	uint8_t color = 0;
	do {
		fill_screen(color);
		busy_wait(0xFFFFF);
		color = (color++) % 255;
	} while(1);
}

/*---------------------------------------------------------------------------*/
uint8_t flashrom_erase(uint8_t *addr)
{
    uint8_t istate = prepare();

    FCTL3 = FWKEY;               /* Lock = 0 */
    busy_wait();
    FCTL1 = FWKEY | ERASE;
    *addr  = 0;                    /* erase Flash segment */
    busy_wait();
    FCTL1 = FWKEY;               /* ERASE = 0 */
    FCTL3 = FWKEY | LOCK;
    finish(istate);
    return 1;
}

void calibrate_delay(void)
{
    unsigned long tmp, loopbit;
    int lps_precision = LPS_PREC;

    loops_per_tick = (1<<12);

    printk("Calibrating delay... ");

    while (loops_per_tick <<= 1) {
        /* wait for "start of" clock tick */
        tmp = g_timer_ticks;
        while (tmp == g_timer_ticks)
            /* nothing */;
        /* Go .. */
        tmp = g_timer_ticks;
        busy_wait(loops_per_tick);

        tmp = g_timer_ticks - tmp;
        if (tmp)
            break;
    }

    /* Do a binary approximation to get loops_per_tick set to equal one clock
     * (up to lps_precision bits)
     */
    loops_per_tick >>= 1;
    loopbit = loops_per_tick;
    while ( lps_precision-- && (loopbit >>= 1) ) {
        loops_per_tick |= loopbit;

        /* wait for "start of" clock tick */
        tmp = g_timer_ticks;
        while (tmp == g_timer_ticks)
            /* nothing */;
        /* Go .. */
        tmp = g_timer_ticks;
        busy_wait(loops_per_tick);

        if (g_timer_ticks != tmp)   /* longer than 1 tick */
            loops_per_tick &= ~loopbit;
    }

    printk ("%u loops per second (%lu.%02lu BogoMIPS)\r\n",
            loops_per_tick * HZ,
            loops_per_tick/(500000/HZ),
            loops_per_tick/(5000/HZ) % 100);
}

/* 获得硬盘参数信息 */
static void identify_disk(struct disk* hd) {
	char id_info[512];
	select_disk(hd);
	cmd_out(hd->my_channel, CMD_IDENTIFY);
	/* 向硬盘发送指令后便通过信号量阻塞自己,
	 * 待硬盘处理完成后,通过中断处理程序将自己唤醒 */
	sema_down(&hd->my_channel->disk_done);

	/* 醒来后开始执行下面代码*/
	if (!busy_wait(hd)) {     //  若失败
		char error[64];
		sprintf(error, "%s identify failed!!!!!!\n", hd->name);
		PANIC(error);
	}
	read_from_sector(hd, id_info, 1);

	char buf[64];
	uint8_t sn_start = 10 * 2, sn_len = 20, md_start = 27 * 2, md_len = 40;
	swap_pairs_bytes(&id_info[sn_start], buf, sn_len);
	printk("   disk %s info:\n      SN: %s\n", hd->name, buf);
	memset(buf, 0, sizeof(buf));
	swap_pairs_bytes(&id_info[md_start], buf, md_len);
	printk("      MODULE: %s\n", buf);
	uint32_t sectors = *(uint32_t*)&id_info[60 * 2];
	printk("      SECTORS: %d\n", sectors);
	printk("      CAPACITY: %dMB\n", sectors * 512 / 1024 / 1024);
}

void shared_mutex::imp_lock_shared(u32 val)
{
	verify("shared_mutex underflow" HERE), val < c_err;

	for (int i = 0; i < 10; i++)
	{
		busy_wait();

		if (try_lock_shared())
		{
			return;
		}
	}

	// Acquire writer lock and downgrade
	const u32 old = m_value.fetch_add(c_one);

	if (old == 0)
	{
		lock_downgrade();
		return;
	}

	verify("shared_mutex overflow" HERE), (old % c_sig) + c_one < c_sig;
	imp_wait();
	lock_downgrade();
}

void config_lcd_display() {

	clear_rs();
	clear_rw();
	clear_e();

	rs_output();
	rw_output();
	e_output();
	config_bus_output();
	
	_delay_ms(15);
	output_nibble(0x30);
	_delay_ms(6);
	output_nibble(0x30);
	_delay_ms(2);
	output_nibble(0x30);
	_delay_ms(2);
	output_nibble(0x20);

	busy_wait();
	
	write_command_lcd(0x28);
	write_command_lcd(0x08);
	write_command_lcd(0x01);
	write_command_lcd(0x06);
}	

void poll_avail(void)
{
	unsigned head = host.used_idx;
#ifdef RING_POLL
	for (;;) {
		unsigned index = ring.avail->ring[head & (ring_size - 1)];
		if ((index ^ head ^ 0x8000) & ~(ring_size - 1))
			busy_wait();
		else
			break;
	}
#else
	while (ring.avail->idx == head)
		busy_wait();
#endif
}

static void
flash_erase_sectors(int start, int cnt)
{
	int addr, sum, i;

	addr = start * (FLASH_BLOCKLEN / 256);
	for (; cnt > 0; cnt--, addr += (FLASH_BLOCKLEN / 256)) {
		/* Skip already blank sectors */
		spi_start_transaction(IO_SPI_FLASH);
		spi_byte(IO_SPI_FLASH, SPI_CMD_FASTRD);
		spi_byte(IO_SPI_FLASH, addr >> 8);
		spi_byte(IO_SPI_FLASH, addr);
		spi_byte(IO_SPI_FLASH, 0);
		spi_byte(IO_SPI_FLASH, 0); /* dummy byte, ignored */
		for (i = 0, sum = 0xff; i < FLASH_BLOCKLEN; i++)
			sum &= spi_byte(IO_SPI_FLASH, 0);
		if (sum == 0xff)
			continue;

		/* Write enable */
		spi_start_transaction(IO_SPI_FLASH);
		spi_byte(IO_SPI_FLASH, SPI_CMD_WREN);
		spi_start_transaction(IO_SPI_FLASH);
		spi_byte(IO_SPI_FLASH, SPI_CMD_ERSEC);
		spi_byte(IO_SPI_FLASH, addr >> 8);
		spi_byte(IO_SPI_FLASH, addr);
		spi_byte(IO_SPI_FLASH, 0);
		busy_wait();
	}
}

void gpio_init(void)
{
    SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK | SIM_SCGC5_PORTE_MASK;
    // configure pin as GPIO
    PIN_HID_LED_PORT->PCR[PIN_HID_LED_BIT] = PORT_PCR_MUX(1);
    PIN_MSC_LED_PORT->PCR[PIN_MSC_LED_BIT] = PORT_PCR_MUX(1);
    PIN_CDC_LED_PORT->PCR[PIN_CDC_LED_BIT] = PORT_PCR_MUX(1);
    PIN_SW_RESET_PORT->PCR[PIN_SW_RESET_BIT] = PORT_PCR_MUX(1);
    PIN_POWER_EN_PORT->PCR[PIN_POWER_EN_BIT] = PORT_PCR_MUX(1);
    // led off
    gpio_set_hid_led(GPIO_LED_OFF);
    gpio_set_cdc_led(GPIO_LED_OFF);
    gpio_set_msc_led(GPIO_LED_OFF);
    // power regulator on
    PIN_POWER_EN_GPIO->PDOR |= PIN_POWER_EN;
    // set as output
    PIN_HID_LED_GPIO->PDDR |= PIN_HID_LED;
    PIN_MSC_LED_GPIO->PDDR |= PIN_MSC_LED;
    PIN_CDC_LED_GPIO->PDDR |= PIN_CDC_LED;
    PIN_POWER_EN_GPIO->PDDR |= PIN_POWER_EN;
    // set as input
    PIN_SW_RESET_GPIO->PDDR &= ~PIN_SW_RESET;

    // Let the voltage rails stabilize.  This is especailly important
    // during software resets, since the target's 3.3v rail can take
    // 20-50ms to drain.  During this time the target could be driving
    // the reset pin low, causing the bootloader to think the reset
    // button is pressed.
    // Note: With optimization set to -O2 the value 1000000 delays for ~85ms
    busy_wait(1000000);
}

/* Busy-wait for approximately NUM/DENOM seconds. */
static void
real_time_delay (int64_t num, int32_t denom)
{
  /* Scale the numerator and denominator down by 1000 to avoid
     the possibility of overflow. */
  ASSERT (denom % 1000 == 0);
  busy_wait (loops_per_tick * num / 1000 * TIMER_FREQ / (denom / 1000));
}

int main(void)
{
        Timer0_pwm_int(10);
        enable_int1();
        enable_int();
        busy_wait();
        return 0;
}

 void task1_code()
  {
  /* Custom Code */
    printf("hello task 1!\n");
    if(task_index == 1 /*&& !sp_executed*/){ sp_executed = true; sp_task_request(); }
    task_index++;
    task_index = task_index%2;
    busy_wait(EXECUTIVE_QUANT * 0.9);
  }

/*
 * This is the function that does the actual transmission of the
 * frame. It sends the data to the driver which will then send it
 * over the air. If the channel is busy, then it will back off for
 * a random delay and then retry the transfer. If it exceeds the
 * maximum backoffs, it will abort the transmission and send a data
 * confirm with a failure status. After transmission, if no ack is
 * needed, then a data confirm will immediately get issued to the
 * next higher layer that requested the transmission. If an ack is
 * required, the data confirm won't be sent until a proper ack is received.
 */
void mac_out(buffer_t *buf, bool ack_req, U8 dsn, U8 handle)
{
	U8 i;
	U16 csma_time;
	mac_pib_t *pib = mac_pib_get();
	mac_pcb_t *pcb = mac_pcb_get();

	for (i = 0; i < aMaxCsmaBackoffs; i++)
	{
		/* check if the channel is clear */
		if (drvr_get_cca())
		{
			/* send the frame if the CCA clears */
			drvr_tx(buf);

			/* collect a transmission stat here */
			pcb->total_xmit++;

			/*
			 * if there's no ack request,
			 * then we're finished. free the buf.
			 */
			if (!ack_req) {
				mac_data_conf(MAC_SUCCESS, handle);
				buf_free(buf);
			}
			return;
		} else {
			/*
			 * channel busy. do a busy wait and try again.
			 * Shift left of signed quantity (int) due to
			 * left shift of constant "1". its okay.
			 */
			csma_time = drvr_get_rand() % (U16)((1 << pib->min_be) - 1);
			busy_wait(csma_time);
		}
	}

	/*
	 * exceeded max csma backoffs. clean up and send a
	 * confirm with a fail message.
	 */
	if (ack_req)
	{
		/*
		 * if the ack request is set, then we need to
		 * check the retry queue and remove the entry.
		 */
		mac_retry_rem(dsn);

		/* collect a transmit fail stat here */
		pcb->total_fail++;
	} else
		buf_free(buf);

	mac_data_conf(MAC_CHANNEL_ACCESS_FAILURE, handle);
}

void flashrom_write(uint8_t *dst, uint8_t *src, size_t size)
{
    unsigned int i;
    FCTL3 = FWKEY;              /* Lock = 0 */
    busy_wait();

    for(i = size; i > 0; i--) {
        FCTL1 = FWKEY | WRT;
        *dst = *src;                /* program Flash word */

        while(!(FCTL3 & WAIT)) {
            nop();
        }
    }

    busy_wait();
    FCTL1 = FWKEY;              /* WRT = 0 */
    FCTL3 = FWKEY | LOCK;              /* Lock = 1 */
}

/*!
 * @brief the entry point of this program
 */
int
main(void)
{
  init();
  for (;;) {
    led_out(((~(*TMDR_PIO_PDSR & DSW_MASK)) >> 11) & LED_MASK);
    busy_wait();
  }
  return 0;
}

void shared_mutex::imp_lock_unlock()
{
	u32 _max = 1;

	for (int i = 0; i < 30; i++)
	{
		const u32 val = m_value;

		if (val % c_one == 0 && (val / c_one < _max || val >= c_sig))
		{
			// Return if have cought a state where:
			// 1) Mutex is free
			// 2) Total number of waiters decreased since last check
			// 3) Signal bit is set (if used on the platform)
			return;
		}

		_max = val / c_one;

		busy_wait(1500);
	}

#ifndef _WIN32
	while (false)
	{
		const u32 val = m_value;

		if (val % c_one == 0 && (val / c_one < _max || val >= c_sig))
		{
			return;
		}

		if (val <= c_one)
		{
			// Can't expect a signal
			break;
		}

		_max = val / c_one;

		// Monitor all bits except c_sig
		futex(reinterpret_cast<int*>(&m_value.raw()), FUTEX_WAIT_BITSET_PRIVATE, val, nullptr, nullptr, c_sig - 1);
	}
#endif

	// Lock and unlock
	if (!m_value.fetch_add(c_one))
	{
		unlock();
		return;
	}

	imp_wait();
	unlock();
}