bool turn_left(int speed)
{
  int portA = ROBOT_WHEEL_LEFT_PORT;
  int portB = ROBOT_WHEEL_RIGHT_PORT;
  bool keep_turning2; 
  int forward_distance = command_get_forward_distance_mm();
  
  motors_set_speed(portA, -30);
  motors_set_speed(portB, 30+speed);
  sleep_ms(500);

    forward_distance = command_get_forward_distance_mm();

    if ( forward_distance < 200)  //Tune this parameter!!!!
    { 
      keep_turning2 = true;
    }
    else
    {
      sleep_ms(800); // TURN BIT MORE TO MAKE SURE RIGHT SENSOR MEASURES WALL CORRECTLY
      keep_turning2 = false;
    }
  
  return keep_turning2;
}

void *send_function(void *arg)
{
    int sockfd,val,len,i=0;

    char buffer[MAX_PKT_SIZE] = {"welcome to linux hello\0"};  //MAX_PKT_SIZE
    
    //接收线程detach自己
    pthread_detach(pthread_self());
    
    sockfd = *( (int*)arg );
    printf("[send_function]sockfd:%d \n",sockfd);
    
    val = 1;
    len = sizeof(int);
    Setsockopt(sockfd, SOL_TCP, TCP_NODELAY,(void *)&val, len);
    
    
    //根据不同测试场景需要选择是否设置sndbuf
    val = 40000;
    len = sizeof(int);
    Setsockopt(sockfd, SOL_SOCKET, SO_SNDBUF,(void *)&val, len);
    
    sleep_ms(1000-3);
    while(i<17)
    {
        sleep_ms(3);
        Write(sockfd,buffer,50);
        
        i++;
    }
    //sleep_ms(50);
    //Write(sockfd,buffer,100);

    return 0;
}

void *thread_main8(void *arg)
{
  /*  The mutator will patch in messaging primitives to signal events at mutex creation,
      deletion, locking and unlocking.  Thus far, we are only considering reporting of events
      so actual contention is meaningless */
  Lock_t newmutex;
  if (!createLock(&newmutex)) {
     fprintf(stderr, "%s[%d]:  createLock failed\n", __FILE__, __LINE__);
     return NULL;
  }
  sleep_ms(100);
  if (!lockLock(&newmutex)) {
     fprintf(stderr, "%s[%d]:  lockLock failed\n", __FILE__, __LINE__);
     return NULL;
  }
  sleep_ms(100);
  if (!unlockLock(&newmutex)) {
     fprintf(stderr, "%s[%d]:  unlockLock failed\n", __FILE__, __LINE__);
     return NULL;
  }
  sleep_ms(100); 
  if (!destroyLock(&newmutex)) {
     fprintf(stderr, "%s[%d]:  destroyLock failed\n", __FILE__, __LINE__);
     return NULL;
  }

  sleep(1);
  return NULL;
}

static NTSTATUS sys_thread(void *null)
{
	VCPU_DEBUG_RAW("waiting a bit\n");
	sleep_ms(2000);

	int m = ksm_hook_page(MmMapLockedPagesSpecifyCache, hk_MmMapLockedPagesSpecifyCache);
	if (m >= 0) {
		VCPU_DEBUG("hooked: %d\n", m);
		if (MmMapLockedPagesSpecifyCache((PMDLX)0xdeadbeef,
						 KernelMode,
						 MmNonCached,
						 (PVOID)0x00000000,
						 TRUE,
						 NormalPagePriority) == (PVOID)0xbaadf00d)
			VCPU_DEBUG_RAW("We succeeded\n");
		else
			VCPU_DEBUG_RAW("we failed\n");
		sleep_ms(2000);

		/* Trigger #VE  */
		struct page_hook_info *phi = ksm_find_hook(m);
		u8 *r = (u8 *)(uintptr_t)MmMapLockedPagesSpecifyCache;
		VCPU_DEBUG("Equality: %d\n", memcmp(r, phi->data, phi->size));
		return ksm_unhook_page(m);
	}

	return -m;
}

void enc28j60_arch_spi_init(void) {
  enc28j60_arch_spi_deselect();
  HAL_GPIO_WritePin(PIN_ENC28J60_RESET_PORT, PIN_ENC28J60_RESET_PIN, GPIO_PIN_RESET);
  sleep_ms(50);
  HAL_GPIO_WritePin(PIN_ENC28J60_RESET_PORT, PIN_ENC28J60_RESET_PIN, GPIO_PIN_SET);
  sleep_ms(1000);
}

void lcd_init(const lcd_def *def)
{
  // Set GPIO clock
  if (def->port == GPIOA)
  {
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
  }
  else if (def->port == GPIOB)
  {
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
  }
  else if (def->port == GPIOC)
  {
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
  }
  else if (def->port == GPIOD)
  {
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOD, ENABLE);
  }
  else if (def->port == GPIOE)
  {
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOE, ENABLE);
  }
  else if (def->port == GPIOF)
  {
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOF, ENABLE);
  }

  // Initialize GPIO pins
  GPIO_InitTypeDef gpio_init;

  gpio_init.GPIO_Pin   = def->pin_register_select |
                         def->pin_enable |
                         def->pin_d4 |
                         def->pin_d5 |
                         def->pin_d6 |
                         def->pin_d7;

  gpio_init.GPIO_Mode  = GPIO_Mode_OUT;
  gpio_init.GPIO_OType = GPIO_OType_PP;
  gpio_init.GPIO_Speed = GPIO_Speed_10MHz;
  gpio_init.GPIO_PuPd  = GPIO_PuPd_NOPULL;

  GPIO_Init(def->port, &gpio_init);

  // wait for initialization
  sleep_ms(30);

  // select 4-bit interface
  _lcd_set_nibble(def, 2);
  sleep_ms(20);

  // init
  _lcd_send_command(def, 0x28); // 4 bit mode, 2 lines, 5x7 glyph
  _lcd_send_command(def, 0x0c); // display with no cursor, no blink
  _lcd_send_command(def, 0x06); // automatic increment, no display shift

  lcd_home(def);
  lcd_clear(def);
}

void move_away_from_wall()
{
     motors_set_speed(ROBOT_WHEEL_LEFT_PORT, 0);
     motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, 50);
     sleep_ms(400);
     motors_set_speed(ROBOT_WHEEL_LEFT_PORT, 50);
     motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, 0);
     sleep_ms(200);  
}

void CThrower::shoot()
{
  printf("Throw engine SHOOT start\n");
  down();
  printf("SHOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOT");
  motors_set_speed(ROBOT_BALL_THROW_PORT, -100);
  sleep_ms(500);
  motors_stop(ROBOT_BALL_THROW_PORT);
  sleep_ms(100);
  is_up = false;
  is_down = false;
  is_half_down = false;
  printf("Throw engine SHOOT done\n");
}

int
main(int argc, char** argv){
    if (argc != 2) {
        fprintf(stdout, "Usage: %s <file_descriptor>\n", argv[0]);
        return 1;
    }

    int socket_num = atoi(argv[1]);
    if (socket_num <= 0) {
        fprintf(stderr, "Invalid file_descriptor '%s'\n", argv[1]);
        return 1;
    }

    struct sockaddr_in sin;
    sin.sin_family = AF_INET;
    sin.sin_addr.s_addr = inet_addr("172.16.16.104");
    sin.sin_port = htons(31337);

    int s = socket(AF_INET, SOCK_STREAM, 0);

    int res = connect(s, (struct sockaddr*)&sin, sizeof(sin));
    sleep_ms(100);

    int i;
    char buff[4096 * 48];
    for (i = 0; i < sizeof(buff) / sizeof(socket_num); i++)
        memcpy(buff + i * sizeof(socket_num), &socket_num, sizeof(socket_num));
    
    int to_write = sizeof(buff);
    while(to_write > 0) {
        int result = send(s, buff + sizeof(buff) - to_write, 4096, MSG_NOSIGNAL);
        to_write -= result;        
        sleep_ms(200);
    }

    sleep(5);
    
    while(1) {
        int result = recv(s, buff, 4096, 0);
        if (result == 0){
            close(s);
            return;
        }
        else if (result > 0){
            fprintf(stdout, "%d '%s'\n", result, buff);
            memset(buff, 0, sizeof(buff));
        }
    }
    return 0;
}

void *get_info(void *arg)
{
    int sockfd;
    int len,i=0,last_in = 0,last_out = 0;;
    struct tcp_info_user info;
    FILE *stream = fopen("rst_server","w+"); 
    
    //接收线程detach自己
    pthread_detach(pthread_self());  
      
    sockfd = *( (int*)arg );
    
    
            
    i = 0;
    while(i < 100*200000 && openflag>0)
    {
        len = sizeof(info);
        errno = 0;
        Getsockopt(sockfd, SOL_TCP, TCP_INFO,(void *)&info, (socklen_t *)&len);
        if(errno != 0)
        {
            //perror("get sock error\n");
            break;
        }
        if( (last_in != info.tcpi_segs_in) || (last_out != info.tcpi_segs_out) )
        {
            sleep_ms(1);
            len = sizeof(info);
            Getsockopt(sockfd, SOL_TCP, TCP_INFO,(void *)&info, (socklen_t *)&len);
            
            fprintftcpinfo(stream,&info);

            //printf("i=%d\n",i);
            
            
            last_in = info.tcpi_segs_in;
            last_out = info.tcpi_segs_out;
            i++;
            continue;
        }
     
        
        sleep_ms(1);
        i++;
    }  
    fclose(stream);
    return 0;
}

inline void complex_calculation()
{
	LOG(INFO) << "complex_calculation started";
	LOG(INFO) << "Starting time machine...";
	sleep_ms(200);
	LOG(WARNING) << "The flux capacitor is not getting enough power!";
	sleep_ms(400);
	LOG(INFO) << "Lighting strike!";
	VLOG(1) << "Found 1.21 gigawatts...";
	sleep_ms(400);
	std::thread([](){
		LOG(ERROR) << "We ended up in 1985!";
	}).join();
	LOG(INFO) << "complex_calculation stopped";
}

static void restore_rnc(struct asill_s *A)
{
  return;			/* doesn't work */
  set_reg(A, MT9M034_RESET_REGISTER, 0x10d8);
  set_reg(A, MT9M034_COLUMN_CORRECTION, 0x0000);
  set_reg(A, MT9M034_RESET_REGISTER, 0x10dc);
#if 1				/* from DS wait 1 frame for column cor */
  sleep_ms(A, 10 + 2 * A->exposure_us / 1000);
#else
  sleep_ms(A, 51);
#endif
  set_reg(A, MT9M034_RESET_REGISTER, 0x10d8);
  set_reg(A, MT9M034_COLUMN_CORRECTION, 0xe007);
  set_reg(A, MT9M034_RESET_REGISTER, 0x10dc);
}

void
InputProvider::MouseRightClick (unsigned int delay)
{
	LOG_INPUT ("[%i shocker] InputProvider::MouseRightClick (%u)\n", getpid (), delay);
	g_assert (xtest_available);
	g_assert (display);

	XTestFakeButtonEvent (display, 3, true, CurrentTime);
	XFlush (display);
	sleep_ms (delay);

	XTestFakeButtonEvent (display, 3, false, CurrentTime);
	XFlush (display);
	sleep_ms (delay);
}

/**
 * return: TRUE: OK, FALSE: error
 */
gboolean sysfs_set_gps_device_power(gboolean poweron)
{
	int i, fd = -1, count = sizeof(sysfs_gps_power) / sizeof(char *);

	for (i=0; i<count; i++) {
		fd = open(sysfs_gps_power[i], O_WRONLY);
		if (fd > 0)
			break;
	}

	if (fd < 0)
		return -1;

	char c1 = '1', c0 = '0';

	if (poweron) {
		if (write(fd, &c0, 1) != 1)
			return FALSE;
		sleep_ms(500);
	}

	if (write(fd, poweron? &c1 : &c0, 1) != 1)
		return FALSE;

	close(fd);

	if (poweron)
		sleep(3);

	return TRUE;
}

void uart_flush_output()
{
	tcflush(gps_dev_fd, TCOFLUSH);

	char buf[128];

	/* turn to non-blocking mode, consume (possible) ubx binary garbage */
	if (fcntl(gps_dev_fd, F_SETFL, flags|O_NONBLOCK) != 0) {
		log_error("sweep_garbage, change to non-blocking mode failed.");
		return;
	}

	/* important: wait for pending output to be written to GPS output buffer */
	sleep_ms(SEND_RATE);

	int count = 0;
	int n;

	while ((n = read(gps_dev_fd, buf, sizeof(buf))) > 0) {
		count += n;
		/* hack: lots of NMEA to read?
		 * size of any UBX output block should <= 1K */
		if (count > 1024) {
			log_warn("Detect possible conflict on UART.");
			g_context.uart_conflict = TRUE;
			break;
		}
	}

	/* restore flags */
	if (fcntl(gps_dev_fd, F_SETFL, flags) != 0) {
		log_error("sweep_garbage, change to blocking mode failed.");
		return;
	}
}

void MeshUpdateThread::doUpdate()
{
	std::shared_ptr<MeshMakeData> q;
	while ((q = m_queue_in.pop())) {
		try {
		m_queue_in.m_process.set(q->m_blockpos, 1);

		ScopeProfiler sp(g_profiler, "Client: Mesh making " + itos(q->step));

		m_queue_out.push_back(MeshUpdateResult(q->m_blockpos, MapBlock::mesh_type(new MapBlockMesh(q.get(), m_camera_offset))));

		m_queue_in.m_process.erase(q->m_blockpos);

#if _MSC_VER
		sleep_ms(1); // dont overflow gpu, fix lag and spikes on drawtime
#endif

#ifdef NDEBUG
		} catch (BaseException &e) {
			errorstream<<"MeshUpdateThread: exception: "<<e.what()<<std::endl;
		} catch(std::exception &e) {
			errorstream<<"MeshUpdateThread: exception: "<<e.what()<<std::endl;
		} catch (...) {
			errorstream<<"MeshUpdateThread: Ooops..."<<std::endl;
#else
		} catch (int) { //nothing
#endif
		}

	}
}

// Executed inside the worker-thread.
// It is not safe to access V8, or V8 data structures
// here, so everything we need for input and output
// should go on `this`.
void ALCaptureWorker::Execute(const ExecutionProgress& progress)
{
	ALCint			samplesIn = 0;

	alcCaptureStart(device);

	while(*this->capturing)
	{
		alcGetIntegerv(device, ALC_CAPTURE_SAMPLES, 1, &samplesIn);

		if (samplesIn > CAPTURE_SIZE) {

			int sampleSize = CAPTURE_SIZE * sizeof(short);

			char* captured = (char*)malloc(sampleSize);

			alcCaptureSamples(device, captured, CAPTURE_SIZE);

			if (*this->capturing)
			{
				progress.Send(captured, sampleSize);
			}

			delete captured;
		}

		sleep_ms(1);
	}
}

void GUIEngine::run()
{

	// Always create clouds because they may or may not be
	// needed based on the game selected
	video::IVideoDriver* driver = m_device->getVideoDriver();

	cloudInit();

	while(m_device->run() && (!m_startgame) && (!m_kill)) {
		driver->beginScene(true, true, video::SColor(255,140,186,250));

		if (m_clouds_enabled)
		{
			cloudPreProcess();
			drawOverlay(driver);
		}
		else
			drawBackground(driver);

		//drawHeader(driver);
		//drawFooter(driver);

		m_device->getGUIEnvironment()->drawAll();

		driver->endScene();

		if (m_clouds_enabled)
			cloudPostProcess();
		else
			sleep_ms(25);

		m_script->Step();
	}
}

int do_work(ZWay zway) {

    char cmd, cc_cmd, holder_root;
    ZWBYTE dev, inst, cc, cc_val;
    char data_path[256];
    char cmd_buffer[256];

    ZWBOOL was_idle = FALSE;

    ZWBOOL basic_level_attached = FALSE;

    int skip = 0;
    int running = TRUE;
    reset_default_coloring(zway);
    while (running) {
        if (!zway_is_running(zway)) {
            running = FALSE;
            break;
        }

        if (!zway_is_idle(zway)) {
            sleep_ms(10);
            continue;
        } else {
            skip++;
            if (skip = 10) {
                running = FALSE;
            }
        }
    }
}

bool CDevice::SramWrite(uint8_t *buf, int size, bool initCS, bool holdCS)
{
	bool ret = GenericWrite(ID_SPI_SRAM_WRITE, buf, size, initCS, holdCS);

	sleep_ms(50);
	return(ret);
}