#pragma once
// Kinematic body model: where the robot's body is in the world, given only
// where the firmware puts its feet. No dynamics - the body rests on the
// ground plane its feet define, and grounded feet don't slide.
//
// Robot/world frame: right-handed, mm, +X forward, +Y left, +Z up; world
// ground at z = 0 before ground tilt.
#include <stdint.h>

namespace simbody {

void reset();
void setGroundTilt(float roll_deg, float pitch_deg);   // tilts the ground (and so the robot) - robot-gyro tests

// Advances the body to match new body-frame foot positions (18 floats, mm).
void update(const float feet_mm[18], float dt);

// Results.
void position(float out_mm[3]);          // body origin, world
void orientation(float out_wxyz[4]);     // body -> world rotation
float yawDeg();                          // accumulated heading (not wrapped)
int contactMask();                       // bit i = foot i on the ground

// What an IMU fixed to the body reads: accel in g (+1 g up at rest), gyro deg/s.
void imu(float accel_g[3], float gyro_dps[3]);

}  // namespace simbody