#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 ----
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.
constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -45, -90, -135, 135, 90, 45 };
constexpr float LEG_MOUNT_X_MM[NUM_LEGS]      = {  80,   0,  -80,  -80,   0,  80 };
constexpr float LEG_MOUNT_Y_MM[NUM_LEGS]      = { -60, -90,  -60,   60,  90,  60 };

constexpr float COXA_LEN_MM  = 30.0f;
constexpr float FEMUR_LEN_MM = 60.0f;
constexpr float TIBIA_LEN_MM = 100.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 ----
constexpr float STANCE_RADIUS_MM = 140.0f;  // how far out the foot rests
constexpr float STANCE_HEIGHT_MM = 90.0f;   // how far below the coxa pivot the foot rests

// ---- 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;

// ---- 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