void Sys::Error(Str::StringRef message)
{
	// Only try sending an ErrorMsg once
	static std::atomic_flag errorEntered;
	if (!errorEntered.test_and_set()) {
		// Disable checks for sending sync messages when handling async messages.
		// At this point we don't really care since this is an error.
		VM::rootChannel.canSendSyncMsg = true;

		// Try to tell the engine about the error, but ignore errors doing so.
		try {
			VM::SendMsg<VM::ErrorMsg>(message);
		} catch (...) {}
	}

#ifdef BUILD_VM_IN_PROCESS
	// Then engine will close the root socket when it wants us to exit, which
	// will trigger an error in the IPC functions. If we reached this point then
	// we try to exit the thread semi-cleanly by throwing an exception.
	throw ExitException();
#else
	// The SendMsg should never return since the engine should kill our process.
	// Just in case it doesn't, exit here.
	_exit(255);
#endif
}

Terminator::Terminator() {
  static std::atomic_flag created = ATOMIC_FLAG_INIT;
  if (created.test_and_set()) {
    throw std::runtime_error("Terminator may be crated exactly once.");
  }

  std::atomic_init(&this->should_terminate_, false);
  signal_handler_ = SpawnThread(this);
}

namespace atomTest{
	std::atomic_flag lock = ATOMIC_FLAG_INIT;

	void f(int n)
	{
		while(lock.test_and_set(std::memory_order_acquire)){
			cout<<"waiting from thread "<< n <<endl;
		}

		cout<<"thread "<< n <<"starts working!"<<endl;
	}

	void g(int n)
	{
		cout<<"thread "<< n << "is going to start"<<endl;
		lock.clear();
		cout<<"thread "<< n << "starts working"<<endl;
	}

	void test()
	{
		lock.test_and_set();
		thread t1(f, 1);
		thread t2(g, 2);

		t1.join();
		usleep(100); //posix unix
		t2.join();
	}
}

/// <summary>
/// Updates the cover art.
/// </summary>
void Update() {
  // Set while the updater thread is running.
  static std::atomic_flag closed = ATOMIC_FLAG_INIT;

  if (!closed.test_and_set()) {
    std::thread([] () {
      TextFunctions::_Update();
      SendMessage(gLSModule.GetMessageWindow(), WindowMessages::WM_TEXTUPDATENOTIFY, 0, 0);

      for (auto &coverArt : gCoverArt) {
        coverArt.second.Update();
      }

      closed.clear();
    }).detach();
  }
}

// CODETAG_IOR_SIGNALS
//++
// Details: The SIGINT signal is sent to a process by its controlling terminal
// when a
//          user wishes to interrupt the process. This is typically initiated by
//          pressing
//          Control-C, but on some systems, the "delete" character or "break"
//          key can be
//          used.
//          Be aware this function may be called on another thread besides the
//          main thread.
// Type:    Function.
// Args:    vSigno  - (R) Signal number.
// Return:  None.
// Throws:  None.
//--
void sigint_handler(int vSigno) {
#ifdef _WIN32 // Restore handler as it is not persistent on Windows
  signal(SIGINT, sigint_handler);
#endif
  static std::atomic_flag g_interrupt_sent = ATOMIC_FLAG_INIT;
  CMIDriverMgr &rDriverMgr = CMIDriverMgr::Instance();
  lldb::SBDebugger *pDebugger = rDriverMgr.DriverGetTheDebugger();
  if (pDebugger != nullptr) {
    if (!g_interrupt_sent.test_and_set()) {
      pDebugger->DispatchInputInterrupt();
      g_interrupt_sent.clear();
    }
  }

  // Send signal to driver so that it can take suitable action
  rDriverMgr.DeliverSignal(vSigno);
}

static void handler()
{
  // Avoid doing crazy things if we get an uncaught exception inside
  // an uncaught exception
  static std::atomic_flag lock = ATOMIC_FLAG_INIT;
  if (lock.test_and_set()) {
    XBT_ERROR("Multiple uncaught exceptions");
    std::abort();
  }

  // Get the current backtrace and exception
  auto e = std::current_exception();
  auto bt = backtrace();
  try {
    std::rethrow_exception(e);
  }

  // We manage C++ exception ourselves
  catch (std::exception& e) {
    logException(xbt_log_priority_critical, "Uncaught exception", e);
    showBacktrace(bt);
    std::abort();
  }

  // We don't know how to manage other exceptions
  catch (...) {
    // If there was another handler let's delegate to it
    if (previous_terminate_handler)
      previous_terminate_handler();
    else {
      XBT_ERROR("Unknown uncaught exception");
      showBacktrace(bt);
      std::abort();
    }
  }

}

int main(int, char**)
{
#if TEST_STD_VER >= 11
    assert(global.test_and_set() == 1);
#endif
    {
        std::atomic_flag f(false);
        assert(f.test_and_set() == 0);
    }
    {
        std::atomic_flag f(true);
        assert(f.test_and_set() == 1);
    }

  return 0;
}

void append_number(int x)
{
	while (lock_stream.test_and_set()) {
	}
	stream << "thread #" << x << '\n';
	lock_stream.clear();
}

void step()
{
	while(lock_vis.test_and_set()){}
	for(auto i = elements->begin(); i != elements->end(); ++i)
	{
		if(i->step())
		{
			i = elements->erase(i);
			--i;
		}
	}
	lock_vis.clear();
}

void g(int n)
{
    using namespace std::chrono_literals;
    std::this_thread::sleep_for(2s);
    std::cout << "Thread " << n << " is going to clear the flag." << std::endl;
    lock.clear();                                                       // 解锁
}

void* initializeNotifier(void (*process)(uint64_t, void*), void *param, int32_t *status)
{
	if (!process) {
		*status = NULL_PARAMETER;
		return nullptr;
	}
	if (!notifierAtexitRegistered.test_and_set())
		std::atexit(cleanupNotifierAtExit);
	if (notifierRefCount.fetch_add(1) == 0) {
		std::lock_guard<priority_mutex> sync(notifierInterruptMutex);
		// create manager and alarm if not already created
		if (!notifierManager) {
			notifierManager = new tInterruptManager(1 << kTimerInterruptNumber, false, status);
			notifierManager->registerHandler(alarmCallback, NULL, status);
			notifierManager->enable(status);
		}
		if (!notifierAlarm) notifierAlarm = tAlarm::create(status);
	}

	std::lock_guard<priority_recursive_mutex> sync(notifierMutex);
	// create notifier structure and add to list
	Notifier* notifier = new Notifier();
	notifier->prev = nullptr;
	notifier->next = notifiers;
	if (notifier->next) notifier->next->prev = notifier;
	notifier->param = param;
	notifier->process = process;
	notifiers = notifier;
	return notifier;
}

int main()
#endif
{
	gViewerThread = nullptr;
	gRunThread.clear();

	lua_State* L = luaL_newstate();
	luaL_openlibs(L);

	IupOpen(nullptr, nullptr);
	IupControlsOpen();
	iuplua_open(L);
	iupcontrolslua_open(L);

	TER::LoadFunctions(L);
	ZON::LoadFunctions(L);
	MOD::LoadFunctions(L);
	WLD::LoadFunctions(L);
	Viewer::LoadFunctions(L);
	Util::LoadFunctions(L);

	if (luaL_loadfile(L, "gui/main.lua") != 0)
	{
		ShowError("Could not load GUI script:\n%s\n", lua_tostring(L, -1));
	}
	else if (lua_pcall(L, 0, 0, 0) != 0)
	{
		ShowError("Runtime error:\n%s\n", lua_tostring(L, -1));
	}

	lua_close(L);
	return 0;
}

void robotPoseCallback(const wrecs_msgs::sf_state_est::ConstPtr& msg)
{
	if (!lock.test_and_set()) {
		for (int i = 0; i < 30; i++) {
			stored_msg.Joints[i] = msg->joints[i];
		}

		for (int i = 0; i < 3; i++) {
			stored_msg.ComEst[i] = msg->com_est[i];
		}

		for (int i = 0; i < 2; i++) {
			stored_msg.CopEst[i] = msg->cop_est[i];
		}


		stored_msg.FootBF0.X = msg->foot_b_F[0].x;
		stored_msg.FootBF0.Y = msg->foot_b_F[0].y;
		stored_msg.FootBF0.Z = msg->foot_b_F[0].z;

		stored_msg.FootBF1.X = msg->foot_b_F[1].x;
		stored_msg.FootBF1.Y = msg->foot_b_F[1].y;
		stored_msg.FootBF1.Z = msg->foot_b_F[1].z;

		stored_msg.SdfMidFoot0.Position.X = msg->sdf_mid_foot[0].position.x;
		stored_msg.SdfMidFoot0.Position.Y = msg->sdf_mid_foot[0].position.y;
		stored_msg.SdfMidFoot0.Position.Z = msg->sdf_mid_foot[0].position.z;

		stored_msg.SdfMidFoot0.Orientation.W = msg->sdf_mid_foot[0].orientation.w;
		stored_msg.SdfMidFoot0.Orientation.X = msg->sdf_mid_foot[0].orientation.x;
		stored_msg.SdfMidFoot0.Orientation.Y = msg->sdf_mid_foot[0].orientation.y;
		stored_msg.SdfMidFoot0.Orientation.Z = msg->sdf_mid_foot[0].orientation.z;

		stored_msg.SdfMidFoot1.Position.X = msg->sdf_mid_foot[1].position.x;
		stored_msg.SdfMidFoot1.Position.Y = msg->sdf_mid_foot[1].position.y;
		stored_msg.SdfMidFoot1.Position.Z = msg->sdf_mid_foot[1].position.z;

		stored_msg.SdfMidFoot1.Orientation.W = msg->sdf_mid_foot[1].orientation.w;
		stored_msg.SdfMidFoot1.Orientation.X = msg->sdf_mid_foot[1].orientation.x;
		stored_msg.SdfMidFoot1.Orientation.Y = msg->sdf_mid_foot[1].orientation.y;
		stored_msg.SdfMidFoot1.Orientation.Z = msg->sdf_mid_foot[1].orientation.z;

        newMessageArrived = true;

		lock.clear();
	}
}

 void lock() {
     while (m_lock.test_and_set(std::memory_order_acquire)) {
         //spin 
     }
     // test_and_set, atomically sets the flag to true and obtains its previous value 
     // if value is false, set to true, return false, no spin, acquire lock.
     // if value is true, return true, spin... 
 }

int main()
{
    lock.test_and_set();
    std::thread t1(f,1);
    std::thread t2(g,2);

    t1.join();
    t2.join();
}

 /**
  * \param error The error that occured, if any.
  */
 void handleTimeout_(boost::system::error_code const & error) {
     if (!error) {
         std::cout << "Executing random behavior." << std::endl;
         if (!engine_.behavior.queued()) engine_.behavior.enqueueRandom(prefix);
         asyncWaitRandom_();
     } else {
         started_.clear();
     }
 }

void visLoop()
{
	running = true;
	while(running)
	{
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		while(lock_vis.test_and_set()){}
		for(auto i = elements->begin(); i != elements->end(); ++i)
		{
			i->draw();
		}
		lock_vis.clear();

		glfwSwapBuffers(window);
		glfwPollEvents();
	}
}

 /**
  * \param engine The script engine to control.
  * \param prefix The prefix to select behaviors from.
  * \param min The minimum time in milliseconds between two random poses.
  * \param max The maximum time in milliseconds between two random poses.
  */
 PoseChanger(ScriptEngine & engine, std::string prefix, unsigned int min = 2000, unsigned int max = 8000) :
     prefix(prefix),
     min(min),
     max(max),
     engine_(engine),
     timer_(engine_.ios())
 {
     started_.clear();
 }

/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{    
	real_T *isNew = (real_T *)ssGetOutputPortRealSignal(S, 0);
    SL_ROS_SUB_MSG *msg = (SL_ROS_SUB_MSG *)ssGetOutputPortSignal(S, 1);    
	int_T* busInfo = (int_T *)ssGetUserData(S);
    
    //mexPrintf("Acquiring lock...");
	while (lock.test_and_set());
    //mexPrintf("Acquired lock. Outputing message...");
    isNew[0] = (int)newMessageArrived;

    //memcpy(msg, &stored_msg, sizeof(SL_ROS_SUB_MSG));
    if (newMessageArrived) {
		*msg = stored_msg;
        
		/*Copy temporary structure into output bus*/
		/*(void)memcpy(msg + busInfo[0], stored_msg.Joints, busInfo[1]);
		(void)memcpy(msg + busInfo[2], stored_msg.ComEst, busInfo[3]);
		(void)memcpy(msg + busInfo[4], stored_msg.CopEst, busInfo[5]);
		(void)memcpy(msg + busInfo[6], &stored_msg.SdfMidFoot1.Position.X, busInfo[7]);
		(void)memcpy(msg + busInfo[8], &stored_msg.SdfMidFoot1.Position.Y, busInfo[9]);
		(void)memcpy(msg + busInfo[10], &stored_msg.SdfMidFoot1.Position.Z, busInfo[11]);
		(void)memcpy(msg + busInfo[12], &stored_msg.SdfMidFoot1.Orientation.X, busInfo[13]);
		(void)memcpy(msg + busInfo[14], &stored_msg.SdfMidFoot1.Orientation.Y, busInfo[15]);
		(void)memcpy(msg + busInfo[16], &stored_msg.SdfMidFoot1.Orientation.Z, busInfo[17]);
		(void)memcpy(msg + busInfo[18], &stored_msg.SdfMidFoot1.Orientation.W, busInfo[19]);
		(void)memcpy(msg + busInfo[20], &stored_msg.FootBF1.X, busInfo[21]);
		(void)memcpy(msg + busInfo[22], &stored_msg.FootBF1.Y, busInfo[23]);
		(void)memcpy(msg + busInfo[24], &stored_msg.FootBF1.Z, busInfo[25]);
		(void)memcpy(msg + busInfo[26], &stored_msg.FootBF0.X, busInfo[27]);
		(void)memcpy(msg + busInfo[28], &stored_msg.FootBF0.Y, busInfo[29]);
		(void)memcpy(msg + busInfo[30], &stored_msg.FootBF0.Z, busInfo[31]);
		(void)memcpy(msg + busInfo[32], &stored_msg.SdfMidFoot0.Position.X, busInfo[33]);
		(void)memcpy(msg + busInfo[34], &stored_msg.SdfMidFoot0.Position.Y, busInfo[35]);
		(void)memcpy(msg + busInfo[36], &stored_msg.SdfMidFoot0.Position.Z, busInfo[37]);
		(void)memcpy(msg + busInfo[38], &stored_msg.SdfMidFoot0.Orientation.X, busInfo[39]);
		(void)memcpy(msg + busInfo[40], &stored_msg.SdfMidFoot0.Orientation.Y, busInfo[41]);
		(void)memcpy(msg + busInfo[42], &stored_msg.SdfMidFoot0.Orientation.Z, busInfo[43]);
		(void)memcpy(msg + busInfo[44], &stored_msg.SdfMidFoot0.Orientation.W, busInfo[45]);*/
    }
    
    newMessageArrived = false;
    lock.clear();
}

	void test()
	{
		lock.test_and_set();
		thread t1(f, 1);
		thread t2(g, 2);

		t1.join();
		usleep(100); //posix unix
		t2.join();
	}