static void KMeansIterate(TRAININGSET *pTS, CODEBOOK *pCBnew, 
PARTITIONING *pPnew, llong *distance, int quietLevel, int i, int *iter, 
double *time, double *error, int initial)
{
  int       active[BookSize(pCBnew)];
  int       activeCount;
  double    oldError;

  *iter  = 0;
  *error = AverageErrorForSolution(pTS, pCBnew, pPnew, MSE);
  PrintIterationKM(quietLevel, i, *iter, *error, GetClock(*time), pCBnew);

  /* K-means iterations */
  do 
  {
    (*iter)++;
    oldError = *error;

    OptimalRepresentatives(pPnew, pTS, pCBnew, active, &activeCount);
    OptimalPartition(pCBnew, pTS, pPnew, active, activeCount, distance);

    *error = AverageErrorForSolution(pTS, pCBnew, pPnew, MSE);
    PrintIterationKM(quietLevel, i, *iter, *error, GetClock(*time), pCBnew);
  }
  while (*error < oldError);  /* until no improvement */

}  /* KMeansIterate() */

void Shooter::InitDefaultCommand() {
	// Set the default command for a subsystem here.
	//SetDefaultCommand(new MySpecialCommand());
	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	runtoggle = true;
	entryset = 0;
	exitset = 0;
	middleset = 0;
	entryvolt = 0;
	exitvolt = 0;
	middlevolt = 0;
	prevAngle = 0;
	shutoffTimer = GetClock() + 3;
	shootertimer = GetClock();
	EntrySOTimer = 0;
	EntrySOFlag = false;
	MiddleSOTimer = 0;
	MiddleSOFlag = false;
	ExitSOTimer = 0;
	ExitSOFlag = false;
	EntryPrevCurrent = 0;
	ExitPrevCurrent = 0;
	MiddlePrevCurrent = 0;
	firetime = 0;
	shooterRamp = true;
	TriggerStopFlag = false;
	FireFlag = false;
	shootercounter = 0;
	firetimer = GetClock();
}

/*
 * Find the resolution of the high-resolution clock by sampling successive
 * values until a tick boundary, at which point the delta is entered into
 * a table.  An average near the median of the table is taken and returned
 * as the system tick size to eliminate outliers due to descheduling (high)
 * or tv0 not being the "zero" time in a given tick (low).
 *
 * Some trickery is needed to defeat the habit systems have of always
 * incrementing the microseconds field from gettimeofday() results so that
 * no two calls return the same value.  Thus, a "tick boundary" is assumed
 * when successive calls return a difference of more than MINTICK ticks.
 * (For gettimeofday(), this is set to 2 us.)  This catches cases where at
 * most one other task reads the clock between successive reads by this task.
 * More tasks in between are rare enough that they'll get cut off by the
 * median filter.
 *
 * When a tick boundary is found, the *first* time read during the previous
 * tick (tv0) is subtracted from the new time to get microseconds per tick.
 *
 * Suns have a 1 us timer, and as of SunOS 4.1, they return that timer, but
 * there is ~50 us of system-call overhead to get it, so this overestimates
 * the tick size considerably.  On SunOS 5.x/Solaris, the overhead has been
 * cut to about 2.5 us, so the measured time alternates between 2 and 3 us.
 * Some better algorithms will be required for future machines that really
 * do achieve 1 us granularity.
 *
 * Current best idea: discard all this hair and use Ueli Maurer's entropy
 * estimation scheme.  Assign each input event (delta) a sequence number.
 * 16 bits should be more than adequate.  Make a table of the last time
 * (by sequence number) each possibe input event occurred.  For practical
 * implementation, hash the event to a fixed-size code and consider two
 * events identical if they have the same hash code.  This will only ever
 * underestimate entropy.  Then use the number of bits in the difference
 * between the current sequence number and the previous one as the entropy
 * estimate.
 *
 * If it's desirable to use longer contexts, Maurer's original technique
 * just groups events into non-overlapping pairs and uses the technique on
 * the pairs.  If you want to increment the entropy numbers on each keystroke
 * for user-interface niceness, you can do the operation each time, but you
 * have to halve the sequence number difference before starting, and then you
 * have to halve the number of bits of entropy computed because you're adding
 * them twice.
 *
 * You can put the even and odd events into separate tables to close Maurer's
 * model exactly, or you can just dump them into the same table, which will
 * be more conservative.
 */
static PGPUInt32
ranTickSize(void)
{
	int i = 0, j = 0;
	PGPUInt32	diff, d[kNumDeltas];
	timetype	tv0, tv1, tv2;
	
	/*
	 * TODO Get some per-run data to seed the RNG with.
	 * pid, ppid, etc.
	 */
	GetClock(&tv0);
	tv1 = tv0;
	do {
		GetClock(&tv2);
		diff = tickdiff(tv2, tv1);
		if (diff > MINTICK) {
			d[i++] = diff;
			tv0 = tv2;
			j = 0;
		} else if (++j >= 4096)	/* Always getting <= MINTICK units */
			return MINTICK + !MINTICK;
		tv1 = tv2;
	} while (i < kNumDeltas);

	/* Return average of middle 5 values (rounding up) */
	qsort(d, kNumDeltas, sizeof(d[0]), ranCompare);
	diff = (d[kNumDeltas / 2 - 2] + d[kNumDeltas / 2 - 1] +
			d[kNumDeltas / 2] + d[kNumDeltas / 2 + 1] +
			d[kNumDeltas/2 + 2] + 4) / 5;
#if NOISEDEBUG
	fprintf(stderr, "Tick size is %u\n", diff);
#endif
	return diff;
}

uint64 PERFD (typePOS* POSITION, int n)
{
  int i;
  uint64 TOTAL = 0, TIME;
  typeMoveList LM[256], *lm;
  DrawBoard (POSITION);
  TIME = GetClock();
  Mobility (POSITION);
  if (IN_CHECK)
    lm = EvasionMoves (POSITION, LM, 0xffffffffffffffff);
  else
    {
      lm = CaptureMoves (POSITION, LM, POSITION->OccupiedBW);
      lm = OrdinaryMoves (POSITION, lm);
    }
  for (i = 0; i < lm - LM; i++)
    {
      Make (POSITION, LM[i].move);
      Mobility (POSITION);
      if (!ILLEGAL)
	{
	  printf ("%s ",Notate (LM[i].move, STRING1[POSITION->cpu]));
	  PERFT (POSITION, n - 1);
	  TOTAL += CNT[n - 1];
	}
      Undo (POSITION, LM[i].move);
    }
  printf ("TOTAL %lld  moves %ld  time: %lld us\n",
	  TOTAL, lm - LM, GetClock() - TIME);
  return TOTAL;
}

int PerformKMeans(TRAININGSET *pTS, CODEBOOK *pCB, PARTITIONING *pP,
		  int clus, int repeats, int InitMethod, 
		  int quietLevel, int useInitial)
{
  PARTITIONING  Pnew, Pinit;
  CODEBOOK      CBnew, CBinit;
  llong         distance[BookSize(pTS)];
  llong         distanceInit[BookSize(pTS)];
  double        totalTime, error, currError;
  int           i, better, iter, totalIter;

  SetClock(&totalTime);
  totalIter = 0;
  currError = error = 0;

  if ((clus < 1) || (BookSize(pTS) < clus) || (repeats < 1))
  {
    return 1;   /* clustering failed */
  }

  InitializeSolutions(pTS, pCB, pP, &CBnew, &Pnew, &CBinit, &Pinit, 
      distanceInit, clus, useInitial);

  PrintHeader(quietLevel);

  /* perform repeats time full K-means */
  for (i = 0; i < repeats; i++)
  {
    better = iter = 0;

    GenerateSolution(pTS, &CBnew, &Pnew, &CBinit, &Pinit, distance, 
		     distanceInit, InitMethod, useInitial);          
    KMeansIterate(pTS, &CBnew, &Pnew, distance, quietLevel, i, &iter, 
        &totalTime, &error, useInitial);

    totalIter += iter;

    /* got better result */
    if ((i == 0) || (error < currError)) 
    {
      CopyCodebook(&CBnew, pCB);
      CopyPartitioning(&Pnew, pP);
      currError = error;
      better = 1;
    }

    PrintRepeat(quietLevel, repeats, i, iter, error, GetClock(totalTime), better);
  }

  PrintFooterKM(quietLevel, currError, repeats, GetClock(totalTime), totalIter);

  FreeCodebook(&CBnew);
  FreePartitioning(&Pnew);
  FreeCodebook(&CBinit);
  FreePartitioning(&Pinit);

  return 0;
}  /* PerformKmeans() */

bool Problem_Interface::computeF(const Epetra_Vector& x, Epetra_Vector& FVec, FillType flag)
{
  double t0 = GetClock();
  bool ok = problem.evaluate(F_ONLY, &x, &FVec, NULL);
  double t1 = GetClock();
  ncalls_computeF_++;
  t_ += (t1-t0);
  return ok;
}

// Callback function to display rendering time (Time in seconds)
void RenderTime(double Time){
 static double interval = 0;

 if(GetClock()-interval > 1.0){
  interval = GetClock();
  char s[0x100];
  sprintf(
   s,
   "Zoom = %.3f; Variable = %d; Render time = %.3lf s; Frame-rate = %.3lf",
   Zoom, Variable, Time, 1.0/Time
  );
  Message(s);
 }
}

	void DisabledPeriodic(void)
	{
		static INT32 printSec = (INT32)GetClock() + 1;
		static const INT32 startSec = (INT32)GetClock();

		
		// while disabled, printout the duration of current disabled mode in seconds
		if (GetClock() > printSec) {
			// Move the cursor back to the previous line and clear it.
			printf("\x1b[1A\x1b[2K");
			printf("Disabled seconds: %d\r\n", printSec - startSec);			
			printSec++;
		}
	}

DriveTrain::DriveTrain() : Subsystem("DriveTrain") {
	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
	frontLeftPos = RobotMap::driveTrainFrontLeftPos;
	frontLeftSteer = RobotMap::driveTrainFrontLeftSteer;
	frontLeft = RobotMap::driveTrainFrontLeft;
	frontRightPos = RobotMap::driveTrainFrontRightPos;
	frontRightSteer = RobotMap::driveTrainFrontRightSteer;
	frontRight = RobotMap::driveTrainFrontRight;
	rearLeftPos = RobotMap::driveTrainRearLeftPos;
	rearLeftSteer = RobotMap::driveTrainRearLeftSteer;
	rearLeft = RobotMap::driveTrainRearLeft;
	rearRightPos = RobotMap::driveTrainRearRightPos;
	rearRightSteer = RobotMap::driveTrainRearRightSteer;
	rearRight = RobotMap::driveTrainRearRight;
	frontLeftDrive = RobotMap::driveTrainFrontLeftDrive;
	frontRightDrive = RobotMap::driveTrainFrontRightDrive;
	rearLeftDrive = RobotMap::driveTrainRearLeftDrive;
	rearRightDrive = RobotMap::driveTrainRearRightDrive;
	gyroscope = RobotMap::driveTrainGyroscope;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
	
	GyroZeroFlag = false;
	GyroZeroTime = GetClock();
	FLInv = 1;
	FRInv = 1;
	RRInv = 1;
	RLInv = 1;
	driveFront = true;
}

/**
 * Insert this Notifier into the timer queue in right place.
 * WARNING: this method does not do synchronization! It must be called from
 * somewhere
 * that is taking care of synchronizing access to the queue.
 * @param reschedule If false, the scheduled alarm is based on the current time
 * and UpdateAlarm
 * method is called which will enable the alarm if necessary.
 * If true, update the time by adding the period (no drift) when rescheduled
 * periodic from ProcessQueue.
 * This ensures that the public methods only update the queue after finishing
 * inserting.
 */
void Notifier::InsertInQueue(bool reschedule) {
  if (reschedule) {
    m_expirationTime += m_period;
  } else {
    m_expirationTime = GetClock() + m_period;
  }
  if (m_expirationTime > Timer::kRolloverTime) {
    m_expirationTime -= Timer::kRolloverTime;
  }
  if (timerQueueHead == nullptr ||
      timerQueueHead->m_expirationTime >= this->m_expirationTime) {
    // the queue is empty or greater than the new entry
    // the new entry becomes the first element
    this->m_nextEvent = timerQueueHead;
    timerQueueHead = this;
    if (!reschedule) {
      // since the first element changed, update alarm, unless we already plan
      // to
      UpdateAlarm();
    }
  } else {
    for (Notifier **npp = &(timerQueueHead->m_nextEvent);;
         npp = &(*npp)->m_nextEvent) {
      Notifier *n = *npp;
      if (n == nullptr || n->m_expirationTime > this->m_expirationTime) {
        *npp = this;
        this->m_nextEvent = n;
        break;
      }
    }
  }
  m_queued = true;
}

// Also used to reinitialize world.
void EldritchFramework::InitializeWorld(const HashedString& WorldDef,
                                        const bool CreateWorld) {
  XTRACE_FUNCTION;

  PrepareForLoad();

  ShutDownWorld();

  WBWorld::CreateInstance();
  WBWorld::GetInstance()->SetClock(GetClock());

  RegisterForEvents();

  m_World = new EldritchWorld;
  m_World->SetCurrentWorld(WorldDef);
  m_World->Initialize();

  m_Audio3DListener->SetWorld(m_World);

  m_UIManager->RegisterForEvents();

  if (m_Game) {
    m_Game->GetBank()->RegisterForEvents();
    m_Game->GetPersistence()->RegisterForEvents();
  }

  if (CreateWorld) {
    m_World->Create();
    InitializeTools();
  }
}

	void Application::Run()
	{
		while (!_quit && !GetWindow()->ShouldClose())
		{
			ClearScreen();
			_clock->Update();

			float oldTime = _currentTime;
			_currentTime = GetClock()->GetAppTime();

			float elapsedTime = _currentTime - oldTime;
			if (elapsedTime <= FLT_EPSILON)
				elapsedTime = 0.0001f;
			else if (elapsedTime > 0.08f)
				elapsedTime = 0.08f;

			//#ifdef _DEBUG
			//		elapsedTime = 0.05f;
			//#endif

			_window->Update();
			_graphicsDevice->Update();
			_inputManager->PollEvents();
			_stateManager->Update(elapsedTime);
			_imguiManager->Update();
			if (_imguiManager->IsCapturingMouse())
				_inputManager->ConsumeMouseInputs();
			if (_imguiManager->IsCapturingKeyboard())
				_inputManager->ConsumeKeyboardInputs();
			_inputManager->DispatchEvents();

			SwapBuffers();
		}
	}

double CDVDClock::ErrorAdjust(double error, const char* log)
{
  CSingleLock lock(m_critSection);

  double clock, absolute, adjustment;
  clock = GetClock(absolute);

  // skip minor updates while speed adjust is active
  // -> adjusting buffer levels
  if (m_speedAdjust != 0 && error < DVD_MSEC_TO_TIME(100))
  {
    return 0;
  }

  adjustment = error;

  if (m_vSyncAdjust != 0)
  {
    if (error > 0.5 * m_frameTime)
      adjustment = m_frameTime;
    else if (error < -0.5 * m_frameTime)
      adjustment = -m_frameTime;
    else
      adjustment = 0;
  }

  if (adjustment == 0)
    return 0;

  Discontinuity(clock+adjustment, absolute);

  CLog::Log(LOGDEBUG, "CDVDClock::ErrorAdjust - %s - error:%f, adjusted:%f",
                      log, error, adjustment);
  return adjustment;
}

/**
 * ProcessQueue is called whenever there is a timer interrupt.
 * We need to wake up and process the current top item in the timer queue as long
 * as its scheduled time is after the current time. Then the item is removed or 
 * rescheduled (repetitive events) in the queue.
 */
void Notifier::ProcessQueue(tNIRIO_u32 mask, void *params)
{
    Notifier *current;
    while (1)                   // keep processing past events until no more
    {
        CRITICAL_REGION(m_semaphore)
        {
            double currentTime = GetClock();
            current = timerQueueHead;
            if (current == NULL || current->m_expirationTime > currentTime)
            {
                break;          // no more timer events to process
            }
            // need to process this entry
            timerQueueHead = current->m_nextEvent;
            if (current->m_periodic)
            {
                // if periodic, requeue the event
                // compute when to put into queue
                current->InsertInQueue(false);
            }
        }
        END_REGION;

        current->m_handler(current->m_param);   // call the event handler
    }
    // reschedule the first item in the queue
    CRITICAL_REGION(m_semaphore)
    {
        UpdateAlarm();
    }
    END_REGION;
}

/**
 * Insert this Notifier into the timer queue in right place.
 * WARNING: this method does not do synchronization! It must be called from somewhere
 * that is taking care of synchronizing access to the queue.
 * @param updateAlarm If true, the UpdateAlarm method is called which will enable the
 * alarm if necessary. Only updated when called from the interrupt routine. This ensures
 * that the public methods only update the queue after finishing inserting.
 */
void Notifier::InsertInQueue(bool updateAlarm)
{
    tRioStatusCode status = 0;
    m_expirationTime = GetClock() + m_period;
    if (timerQueueHead == NULL
        || timerQueueHead->m_expirationTime >= this->m_expirationTime)
    {
        // the queue is empty or greater than the new entry
        // the new entry becomes the first element
        this->m_nextEvent = timerQueueHead;
        timerQueueHead = this;
        if (updateAlarm)
            UpdateAlarm();      // since the first element changed, update alarm
        wpi_assertCleanStatus(status);
    }
    else
    {
        for (Notifier * n = timerQueueHead; n != NULL; n = n->m_nextEvent)
        {
            if (n->m_nextEvent == NULL
                || n->m_expirationTime > this->m_expirationTime)
            {
                // if the new element goes after the *n element
                this->m_nextEvent = n->m_nextEvent;
                n->m_nextEvent = this;
                break;
            }
        }
    }
    m_queued = true;
}

/*
 * pgpRandomCollectOsData
 *	Add random process and thread performance data noise
 *	to the random pool.
 */
	PGPBoolean
pgpRandomCollectOsData(PGPRandomContext const *rc)
{
	long		gestaltResult;
	timetype 	t;
	const VCB	*curVCB;
	
	GetClock(&t);
	pgpRandomAddBytes(rc, (PGPByte const *)&t, sizeof(t));
	
	if( Gestalt( gestaltLowMemorySize, &gestaltResult ) == noErr )
	{
		/* Add all of lowmem (except first byte) into pool */
		pgpRandomAddBytes( rc, (PGPByte *) 1, gestaltResult - 1 );
	}
	
	/* Add all VCB's into the pool */
	curVCB = (const VCB *) GetVCBQHdr()->qHead;
	while( NULL!=(int)( curVCB ) )
	{
		pgpRandomAddBytes( rc, (PGPByte *) curVCB, sizeof( *curVCB ) );
		
		curVCB = (const VCB *) curVCB->qLink;
	}
	
	return TRUE;
}

void DelayInit(void)
{
    SysTick->CTRL |= SysTick_CTRL_CLKSOURCE_Msk; 
    fac_us = GetClock(kCoreClock);
    fac_us /= 1000000;
    fac_ms = fac_us * 1000;
    SysTick->LOAD = SysTick_LOAD_RELOAD_Msk;
}

 /**
 * @brief  详细初始化PIT模块 推荐使用PIT_QuickInit函数
 * @param  
 * @retval None
 */
void PIT_Init(void)
{
    CLK_EN(CLKTbl, 0);
    /* get clock */
    fac_us = GetClock(kBusClock);
    fac_us /= 1000000;
    PIT->MCR &= ~PIT_MCR_MDIS_MASK;
}

static void FTM1_ISR(void)
{
    uint32_t clock;
    InputCaptureValue = FTM_GetChlCounter(HW_FTM1, HW_FTM_CH0);
    clock = GetClock(kBusClock);
    FTM_SetMoudleCounter(HW_FTM1, 0); /* 复位计数值 */
    InputCaptureValue = (clock/4/InputCaptureValue);  /* 频率 = FTM输入时钟/分频/计数值 */
}

	EncoderPIDSource(Encoder *pSensor, float *maxSpeed) 
	{
		m_encoder = pSensor;
		m_maxSpeed = maxSpeed;
		m_ticks = m_encoder->Get();
		m_time = GetClock();
		m_nCount = 0;
	}