#pragma once
#include <Arduino.h>

// =====================================================================
// Everything in this file is a placeholder for a typical small hexapod.
// Measure your own robot and edit these before trusting it on the floor.
// =====================================================================

// ---- Leg geometry ----
// Taken from the Unity model (Hexapod_Simulation_Unity/Assets/Main.unity,
// 1 Unity unit = 10 mm): hip pivots at x = +-10 / 0, z = +-5 / +-6.5;
// coxa 5.5, femur 10, tibia 15. BENCH: confirm against the real chassis.
constexpr int NUM_LEGS = 6;
// Leg order: 0=front-right, 1=mid-right, 2=rear-right, 3=rear-left, 4=mid-left, 5=front-left
// Angles measured counterclockwise from the body's +X (forward) axis, viewed from above.
// This is the direction the coxa points at servo center: straight sideways on this chassis.
constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -90, -90, -90,  90,  90,  90 };
constexpr float LEG_MOUNT_X_MM[NUM_LEGS]      = { 100,   0, -100, -100,  0, 100 };
constexpr float LEG_MOUNT_Y_MM[NUM_LEGS]      = { -50, -65,  -50,   50,  65,  50 };

constexpr float COXA_LEN_MM  = 55.0f;
constexpr float FEMUR_LEN_MM = 100.0f;
constexpr float TIBIA_LEN_MM = 150.0f;

// ---- Joint limits, degrees ----
constexpr float COXA_MIN_DEG  = -60,  COXA_MAX_DEG  = 60;
constexpr float FEMUR_MIN_DEG = -80,  FEMUR_MAX_DEG = 80;
constexpr float TIBIA_MIN_DEG = -150, TIBIA_MAX_DEG = -20;

// ---- Neutral stance, relative to each leg's own coxa pivot ----
// From the Unity model's rest foot point: 130 mm out from the coxa pivot
// (= COXA_LEN_MM + 75), 150 mm below it.
constexpr float STANCE_RADIUS_MM = 75.0f;   // how far out the foot rests, beyond the coxa
constexpr float STANCE_HEIGHT_MM = 150.0f;  // how far below the coxa pivot the foot rests (default height)
// The remote's height slider (CmdPacket.body_height) moves the standing
// height by up to this much either way (100..200 mm here - both ends checked
// reachable in the simulation), at no more than STANCE_HEIGHT_RATE_MM_S.
constexpr float STANCE_HEIGHT_RANGE_MM  = 50.0f;
constexpr float STANCE_HEIGHT_RATE_MM_S = 60.0f;

// ---- Leg collision ----
// Minimum distance between the center lines of neighbouring legs' segments
// (coxa, femur, tibia). Gait strides and body poses are scaled down to keep
// it. BENCH: set to roughly the width of a leg (servo body + bracket).
constexpr float LEG_CLEARANCE_MM = 30.0f;

// ---- Servo pins (Teensy 4.0). Verify these are PWM-capable on your board. ----
// Order per leg: coxa, femur, tibia.
constexpr uint8_t SERVO_PINS[NUM_LEGS][3] = {
  {  2,  3,  4 },   // leg 0
  {  5,  6,  7 },   // leg 1
  {  8,  9, 10 },   // leg 2
  { 11, 12, 24 },   // leg 3
  { 25, 26, 27 },   // leg 4
  { 28, 29, 30 },   // leg 5
};
// Microseconds at 0deg and per additional degree. Calibrate per servo - they vary.
constexpr int SERVO_US_AT_0DEG = 1500;
constexpr int SERVO_US_PER_DEG = 10;

// ---- Gait timing ----
constexpr float DEFAULT_CYCLE_SEC = 0.8f;   // one full gait cycle at neutral speed
constexpr float SWING_LIFT_MM     = 35.0f;

// ---- Control loop ----
constexpr float CONTROL_HZ = 60.0f;

// ---- Radio (nRF24L01) ----
// Address/channel/data rate/PA/payload size live in shared/HexapodProtocol
// so the robot and remote can't drift apart. Only this board's wiring is here.
constexpr uint8_t RADIO_CE_PIN     = 33;
constexpr uint8_t RADIO_CSN_PIN    = 34;
constexpr float   LINK_TIMEOUT_SEC = 0.3f;

// ---- Robot-gyro self-leveling ----
// Integral gain (1/s): a leftover tilt of 1 deg changes the correction by
// this many deg/s. Higher levels faster but can wobble. BENCH: tune on the robot.
constexpr float ROBOT_GYRO_KI           = 3.0f;
constexpr float ROBOT_GYRO_MAX_CORR_DEG = 15.0f;   // steepest slope it will level out

// ---- Setpoint smoothing rates ----
constexpr float POSE_TILT_MAX_RATE_DEG  = 120.0f;  // roll/pitch/yaw offset, deg/s
constexpr float POSE_OFFSET_MAX_RATE_MM = 150.0f;  // X/Y/Z offset, mm/s