void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
    gyroUpdate();

    if (sensors(SENSOR_ACC)) {
        updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
        imuCalculateEstimatedAttitude();
    } else {
        accADC[X] = 0;
        accADC[Y] = 0;
        accADC[Z] = 0;
    }
}

void taskProcessGPS(void)
{
    // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
    // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
    // change this based on available hardware
    if (feature(FEATURE_GPS)) {
        gpsThread();
    }

    if (sensors(SENSOR_GPS)) {
        updateGpsIndicator(currentTime);
    }
}

void handleFrSkyTelemetry(void)
{
    if (!canSendFrSkyTelemetry()) {
        return;
    }

    uint32_t now = millis();

    if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
        return;
    }

    lastCycleTime = now;

    cycleNum++;

    // Sent every 125ms
    sendAccel();
    sendVario();
    sendTelemetryTail();

    if ((cycleNum % 4) == 0) {      // Sent every 500ms
        sendBaro();
        sendHeading();
        sendTelemetryTail();
    }

    if ((cycleNum % 8) == 0) {      // Sent every 1s
        sendTemperature1();

        if (feature(FEATURE_VBAT)) {
            sendVoltage();
            sendVoltageAmp();
            sendAmperage();
            sendFuelLevel();
        }

#ifdef GPS
        if (sensors(SENSOR_GPS))
            sendGPS();
#endif

        sendTelemetryTail();
    }

    if (cycleNum == 40) {     //Frame 3: Sent every 5s
        cycleNum = 0;
        sendTime();
        sendTelemetryTail();
    }
}

static void taskUpdateRxMain(timeUs_t currentTimeUs)
{
    processRx(currentTimeUs);
    isRXDataNew = true;

#if !defined(BARO) && !defined(SONAR)
    // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
    updateRcCommands();
#endif
    updateArmingStatus();

#ifdef BARO
    if (sensors(SENSOR_BARO)) {
        updateAltHoldState();
    }
#endif

#ifdef SONAR
    if (sensors(SENSOR_SONAR)) {
        updateSonarAltHoldState();
    }
#endif
}

void imuInit(void)                                                            // Initialize & precalculate some values here
{
    if (cfg.gy_smrll || cfg.gy_smptc || cfg.gy_smyw)
    {
        SmoothingFactor[ROLL]  = cfg.gy_smrll;
        SmoothingFactor[PITCH] = cfg.gy_smptc;
        SmoothingFactor[YAW]   = cfg.gy_smyw;
        GyroSmoothing          = true;
    } else GyroSmoothing = false;

#ifdef MAG
    if (sensors(SENSOR_MAG)) Mag_init();
#endif
}

void taskUpdateRxMain(void)
{
    processRx();
    isRXDataNew = true;

#if !defined(BARO) && !defined(SONAR)
    // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
    updateRcCommands();
#endif
    updateLEDs();

#ifdef BARO
    if (sensors(SENSOR_BARO)) {
        updateAltHoldState();
    }
#endif

#ifdef SONAR
    if (sensors(SENSOR_SONAR)) {
        updateSonarAltHoldState();
    }
#endif
}

void taskUpdateRxMain(void)
{
    processRx();
    isRXDataNew = true;

#ifdef BARO
    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
    if (haveProcessedAnnexCodeOnce) {
        if (sensors(SENSOR_BARO)) {
            updateAltHoldState();
        }
    }
#endif

#ifdef SONAR
    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
    if (haveProcessedAnnexCodeOnce) {
        if (sensors(SENSOR_SONAR)) {
            updateSonarAltHoldState();
        }
    }
#endif
}

double DetectorModule::stripOccupancyPerEventBarrel() const {
  double rho = center().Rho()/10.;
  double theta = center().Theta();
  double myOccupancyBarrel=(1.63e-4)+(2.56e-4)*rho-(1.92e-6)*rho*rho;
  double factor = fabs(sin(theta))*2; // 2 is a magic adjustment factor
  double dphideta = phiAperture() * etaAperture();
  double minNSegments = minSegments();
  int numStripsAcrossEstimate = sensors().begin()->numStripsAcrossEstimate();
  double modWidth = (maxWidth() + minWidth())/2.;

  double occupancy = myOccupancyBarrel / factor / (90/1e3) * (dphideta / minNSegments) * (modWidth / numStripsAcrossEstimate);

  return occupancy;
}

bool ProcessController::saveSettings(QDomDocument& doc, QDomElement& element)
{
    if(!mProcessList)
        return false;
    element.setAttribute("hostName", sensors().at(0)->hostName());
    element.setAttribute("sensorName", sensors().at(0)->name());
    element.setAttribute("sensorType", sensors().at(0)->type());

    element.setAttribute("version", QString::number(PROCESSHEADERVERSION));
    element.setAttribute("treeViewHeader", QString::fromLatin1(mProcessList->treeView()->header()->saveState().toBase64()));
    element.setAttribute("showTotals", mProcessList->showTotals()?1:0);

    element.setAttribute("units", (int)(mProcessList->units()));
    element.setAttribute("ioUnits", (int)(mProcessList->processModel()->ioUnits()));
    element.setAttribute("ioInformation", (int)(mProcessList->processModel()->ioInformation()));
    element.setAttribute("showCommandLineOptions", mProcessList->processModel()->isShowCommandLineOptions());
    element.setAttribute("showTooltips", mProcessList->processModel()->isShowingTooltips());
    element.setAttribute("normalizeCPUUsage", mProcessList->processModel()->isNormalizedCPUUsage());
    element.setAttribute("filterState", (int)(mProcessList->state()));

    SensorDisplay::saveSettings(doc, element);

    return true;
}

bool
LogFile::saveSettings(QDomDocument& doc, QDomElement& element)
{
	element.setAttribute("hostName", sensors().at(0)->hostName());
	element.setAttribute("sensorName", sensors().at(0)->name());
	element.setAttribute("sensorType", sensors().at(0)->type());

	element.setAttribute("font", monitor->font().toString());

	saveColor(element, "textColor", monitor->palette().color( QPalette::Text ) );
	saveColor(element, "backgroundColor", monitor->palette().color( QPalette::Base ) );

	for (QStringList::Iterator it = filterRules.begin();
		 it != filterRules.end(); ++it)
	{
		QDomElement filter = doc.createElement("filter");
		filter.setAttribute("rule", (*it));
		element.appendChild(filter);
	}

	SensorDisplay::saveSettings(doc, element);

	return true;
}

/* This is equivalent to the act of lifting the robot and moving it
 * to a new location without the encoders registering the movement.
 */
void hyperJump(double new_x, double new_y, double new_o)
{
    robot_x = new_x;
    robot_y = new_y;
    robot_o = new_o;

    /* this function needs a value */
    /* greater than 0 to update the */
    /* position, so this is how long */
    /* a teleportation will take, 1 msec */
    updatePosition(1);

    sensors();
    refreshSimDisplay(TRUE);
    RedrawWholeMap();
}

		static Node::Info tempInfo(std::string uid){

			BB::Node::Sensors sensors({ BB::Node::Sensor("Temperature", BB::Node::Info::Temperature, "C") });

			BB::Node::Settings settings({
				BB::Node::Setting("name"),
				BB::Node::Setting("place"),
				BB::Node::Setting("low"),
				BB::Node::Setting("high"),
				BB::Node::Setting("extremelyLow"),
				BB::Node::Setting("extremelyHigh"),
			});

			Node::Info info(uid, Node::Info::Temperature, sensors, settings);
			return info;
		}

/**
 * Read BARO and update alt/vel topic
 *  Function is called at main loop rate, updates happen at reduced rate
 */
static void updateBaroTopic(uint32_t currentTime)
{
    static navigationTimer_t baroUpdateTimer;

    if (updateTimer(&baroUpdateTimer, HZ2US(INAV_BARO_UPDATE_RATE), currentTime)) {
        float newBaroAlt = baroCalculateAltitude();
        if (sensors(SENSOR_BARO) && isBaroCalibrationComplete()) {
            posEstimator.baro.alt = newBaroAlt;
            posEstimator.baro.epv = posControl.navConfig->inav.baro_epv;
            posEstimator.baro.lastUpdateTime = currentTime;
        }
        else {
            posEstimator.baro.alt = 0;
            posEstimator.baro.lastUpdateTime = 0;
        }
    }
}

void computeIMU(void)
{
    static   float LastGyroSmooth[3] = { 0.0f, 0.0f, 0.0f };
    static   int16_t triywavg[4];
    static   uint8_t triywavgpIDX = 0;
    static   uint32_t prevT;
    uint32_t curT;
    uint8_t  axis, i;
    float    flttmp;
    if (MpuSpecial) GETMPU6050();
    else
    {
        gyro.temperature(&telemTemperature1);                                 // Read out gyro temperature
        Gyro_getADC();                                                        // Also feeds gyroData
        if (sensors(SENSOR_ACC)) ACC_getADC();
    }

    curT               = micros();
    ACCDeltaTimeINS    = (float)(curT - prevT) * 0.000001f;                   // ACCDeltaTimeINS is in seconds now
    ACCDeltaTimeINS    = constrain(ACCDeltaTimeINS, 0.0001f, 0.5f);           // Constrain to range 0,1ms - 500ms
    prevT              = curT;

    if(cfg.acc_calibrated) getEstimatedAttitude();                            // acc_calibrated just can turn true if acc present.
    
    if(cfg.mixerConfiguration == MULTITYPE_TRI && cfg.gy_smyw)                // Moving average for yaw in tri mode
    {
        triywavg[triywavgpIDX] = (int16_t)gyroData[YAW]; triywavgpIDX++;
        if (triywavgpIDX == 4) triywavgpIDX = 0;
        flttmp = 0;
        for (i = 0; i < 4; i++) flttmp += triywavg[i];
        gyroData[YAW] = flttmp * 0.25f;
    }

    if (GyroSmoothing)
    {
        for (axis = 0; axis < 3; axis++)
        {
            if (SmoothingFactor[axis] > 1)                                    // Circumvent useless action
            {
                flttmp               = (float)SmoothingFactor[axis];
                gyroData[axis]       = ((LastGyroSmooth[axis] * (flttmp - 1.0f)) + gyroData[axis]) / flttmp;
                LastGyroSmooth[axis] = gyroData[axis];
            }
        }
    }
}

void sendFRSKYTelemetry(void)
{
    static uint32_t lastCycleTime = 0;
    static uint8_t  cycleNum = 0;

    if (currentTimeMS - lastCycleTime >= CYCLETIME)
    {
        lastCycleTime = currentTimeMS;
        cycleNum++;

        // Sent every 125ms
        sendAccel();
        sendTelemetryTail();

        if ((cycleNum % 4) == 0)        // Sent every 500ms
        {
            sendBaro();
            sendHeading();
            sendTelemetryTail();
        }

        if ((cycleNum % 8) == 0)        // Sent every 1s
        {
            sendTemperature1();

            if (feature(FEATURE_VBAT))
            {
                sendVoltage();
                sendVoltageAmp();
            }

            if (sensors(SENSOR_GPS))
                sendGPS();

            sendTelemetryTail();
        }

        if (cycleNum == 40)       //Frame 3: Sent every 5s
        {
            cycleNum = 0;
            sendTime();
            sendTelemetryTail();
        }
    }
}

void GARule::randomRule() {
   int rInt;                           // random integer in case we need it more than once
   int j;                              // variable for scratch use
   
   j = teamBall(Random::randint(-1,1));
   if (j == 1) myBall(Random::randbool());     // have at least some logic in the random rules;
   else myBall(false);                 // if my team doesn't have the ball, then I can't have it

   Thing sens[8];
   for (int i = 0; i < 8; i++){
      rInt = Random::randint(0,5);
      switch(rInt){

         case(0):
	      sens[i] = Nothing;
	      break;
         case(1):
	      sens[i] = MyBot;
	      break;
         case(2):
	      sens[i] = OtherBot;
	      break;
         case(3):
	      sens[i] = TheBall;
	      break;
         case(4):
	      sens[i] = Net;
	      break;
         case(5):
	      sens[i] = Wall;
	      break;

      }//end switch
   }//end for
   sensors(sens);
   fire(Random::randbool());
   move(Random::randbool());
   rInt = Random::randint(0,2);

   if (rInt == 0)      turn(BotRotation::Left);
   else if (rInt == 1) turn(BotRotation::Right);
   else                turn(BotRotation::None);
   // generates rules with more than one action, but should work anyway since
   // only one action will occur
}

void sendTelemetry(void)
{
    if (mcfg.telemetry_softserial == TELEMETRY_UART && !f.ARMED)
        return;

    if (serialTotalBytesWaiting(core.telemport) != 0)
        return;

    if (millis() - lastCycleTime >= CYCLETIME) {
        lastCycleTime = millis();
        cycleNum++;

        // Sent every 125ms
        sendAccel();
        sendTelemetryTail();

        if ((cycleNum % 4) == 0) {      // Sent every 500ms
            sendBaro();
            sendHeading();
            sendTelemetryTail();
        }

        if ((cycleNum % 8) == 0) {      // Sent every 1s
            sendTemperature1();

            if (feature(FEATURE_VBAT)) {
                sendVoltage();
                sendVoltageAmp();
            }

            if (sensors(SENSOR_GPS))
                sendGPS();

            sendTelemetryTail();
        }

        if (cycleNum == 40) {     //Frame 3: Sent every 5s
            cycleNum = 0;
            sendTime();
            sendTelemetryTail();
        }
    }
}

void RobotMove::GoToPose(Pose newPose){
	xDestination = newPose.getX();
	yDestination = newPose.getY();
	//Vector worldCoordinateVector(xDestination + (*position).GetXPos(), yDestination + (*position).GetYPos());
	//Vector localCoordinateVector(worldCoordinateVector.getDistance(), dtor(worldCoordinateVector.getAngle()) + (*position).GetYaw(), false);
	//xDestination = localCoordinateVector.getXIntensity();//x + (*position).GetXPos();
	//yDestination = localCoordinateVector.getYIntensity();//y + (*position).GetYPos();

	SensorScan sensors(&(*laser), &(*position), &(*worldView), sensorFunction, 1000.0);
	double distance = closenessCutOff;
	while (distance >= closenessCutOff) {				//cout << "Setup";
		(*robot).Read();			
		sensors.ReadSensors();
		Vector sensorVector = sensors.VectorSum();
		sensorVector.invertVector();
		//sensorVector.debugIntensity();
		Vector avoidVector(sensorVector.getDistance(), dtor(sensorVector.getAngle()) + (*position).GetYaw() , false);
		
		Vector driveVector((*position).GetXPos(), (*position).GetYPos(), xDestination, yDestination);
		distance = driveVector.getDistance();
		TargetFunction(driveVector);
		
		//TargetFunction(driveVector);
		//cout << endl << "AvoidVector       : ";
		//avoidVector.debug();avoidVector.debugIntensity();
		//cout << endl << "Drive Vector      : ";
		//cout << closenessCutOff << endl;
		//driveVector.debug();
		//driveVector.debugIntensity();
		//cout << endl;
		driveVector.add(avoidVector);
		//cout << endl << "Resulting Vector  : " << endl;
		//driveVector.debug();driveVector.debugIntensity();
		DriveToVector(driveVector, &(*position));
	}
	Vector directionVector(.1, newPose.getAngle(), true);
	//while (abs((*position).GetYaw() - dtor(newPose.getAngle())) > closenessCutOff) {
	//	DriveToVector(directionVector, &(*position));
	//}
	(*position).SetSpeed(0,0);
	

}

static bool processBinaryModeRequest(uint8_t address)
{
    switch (address) {
#ifdef USE_GPS
    case 0x8A:
        if (sensors(SENSOR_GPS)) {
            hottPrepareGPSResponse(&hottGPSMessage);
            hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
            return true;
        }
        break;
#endif
    case 0x8E:
        hottPrepareEAMResponse(&hottEAMMessage);
        hottQueueSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage));
        return true;
    }

    return false;
}

static void cliStatus(char *cmdline)
{
    uint8_t i;
    uint32_t mask;

    printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
        millis() / 1000, vbat, batteryCellCount);
    mask = sensorsMask();

    uartPrint("Detected sensors: ");
    for (i = 0; ; i++) {
        if (sensorNames[i] == NULL)
            break;
        if (mask & (1 << i))
            printf("%s ", sensorNames[i]);
    }
    if (sensors(SENSOR_ACC))
        printf("ACCHW: %s", accNames[accHardware]);
    uartPrint("\r\n");

    printf("Cycle Time: %d, I2C Errors: %d\r\n", cycleTime, i2cGetErrorCounter());
}