#include "component.hpp"
#include <algorithm>
#include <stdexcept>
namespace asw {
FusedPose fuse(double gx, double gy, double gq, double ox, double oy, double oq) {
  gq = std::max(0.0, gq); oq = std::max(0.0, oq); const double total = gq + oq;
  if (total == 0) throw std::invalid_argument("sensor confidence required");
  return {(gx * gq + ox * oq) / total, (gy * gq + oy * oq) / total, std::min(1.0, total / 2)};
}
}