u32 workLoad = 500;             // Loops per workloadHandler call

void setWorkload(u8 * packet) {
  u32 arg;
  if (packetScanf(packet,"w%d\n",&arg) == 3 && arg < 2000)
    workLoad = arg; 
  logNormal("Workload=%d\n", workLoad);
}

void workloadHandler(u32 when) { // Alarm handler for 'doing work'
  u32 sum = 0;
  for (u32 i = 0; i < workLoad; ++i)   // Work, for us, is just
    sum += random(1000);               // adding up random numbers
  ledSet(BODY_RGB_BLUE_PIN, sum&1);    // and maybe lighting an LED..
  when += 10;                          // Do work every 10ms
  if (IS_EARLIER(when,millis())) when = millis(); // Or ASAP if we're behind
  Alarms.set(Alarms.currentAlarmNumber(),when); // Reschedule
}

void processorMonitor(u32 when) { // Alarm handler for controlling processor speed
  u32 load = Profile.getPPM(TICKER_ALARM_HANDLER)/10000; // Get alarm time, as a percent
  bool lowLoad = load < 33;     // Say under 33% is underloaded
  bool highLoad = load > 66;    // Say over 66% is overloaded

  ledSet(BODY_RGB_RED_PIN,highLoad);   // Display overload condition
  ledSet(BODY_RGB_GREEN_PIN,lowLoad);  // Display underload condition

  u32 speedCode = Processor.getCode(); // Find out how fast we're running now
  if (highLoad && speedCode < Processor.getCodeMax()) // If need and have more..
    ++speedCode;                // More coal to the furnace!  Shovel!
  else if (lowLoad && speedCode > Processor.getCodeMin()) // If need and have less..
    --speedCode;                // Whoa, nellie..

  if (speedCode != Processor.getCode()) {
    Processor.setCode(speedCode);
    logNormal("Load: %2d%%, speed changed to: %dMHz\n",load,Processor.getMHz());
  }

  Profile.begin();                    // Reset statistics gathering
  Alarms.set(Alarms.currentAlarmNumber(),millis()+1000); // Do monitoring about 1Hz
}

void setup() {
  Profile.begin();              // Start profiling
  Body.reflex('w',setWorkload); // Create reflex
  Alarms.set(Alarms.create(workloadHandler),millis());  // Alarm to generate work
  Alarms.set(Alarms.create(processorMonitor),millis()); // Alarm to monitor load
}

void loop() { /* Nothing to do */ }
