#ifndef SERVO_BUS_H
#define SERVO_BUS_H

#include <stdint.h>
#include <stdbool.h>

/*
 * servo_bus.h — Feetech/ST3215 serial bus servo driver (Issue #547)
 *
 * Half-duplex single-wire UART protocol at 1 Mbps.
 * Hardware: USART3 half-duplex on PB10 (USART3_TX, AF7).
 * No separate RX pin — the TX line is bidirectional with HDSEL.
 *
 * Packet format (host to servo):
 *   [0xFF][0xFF][ID][LEN][INSTR][PARAMS...][CKSUM]
 *   CKSUM = ~(ID + LEN + INSTR + PARAMS) & 0xFF
 *
 * Response (servo to host):
 *   [0xFF][0xFF][ID][LEN][ERROR][DATA...][CKSUM]
 *
 * Key ST3215 registers:
 *   0x28  Torque Enable       (1=on, 0=off)
 *   0x2A  Goal Position L     (0-4095)
 *   0x2B  Goal Position H
 *   0x2E  Moving Speed L      (0=max, 1-4095)
 *   0x2F  Moving Speed H
 *   0x38  Present Position L  (0-4095)
 *   0x39  Present Position H
 *   0x3A  Present Speed L     (sign+magnitude: bit15=dir)
 *   0x3B  Present Speed H
 *
 * Position encoding: 0-4095 maps to 0-360 degrees.
 * Center (180 deg) = 2048 raw.
 */

/* ST3215 register addresses */
#define SB_REG_TORQUE_EN    0x28u
#define SB_REG_GOAL_POS_L   0x2Au
#define SB_REG_MOVING_SPD_L 0x2Eu
#define SB_REG_PRES_POS_L   0x38u
#define SB_REG_PRES_SPD_L   0x3Au

/* ST3215 instructions */
#define SB_INSTR_PING       0x01u
#define SB_INSTR_READ       0x02u
#define SB_INSTR_WRITE      0x03u

/* Position encoding */
#define SB_POS_CENTER       2048u   /* 180 deg */
#define SB_POS_MAX          4095u   /* 360 deg */
#define SB_SPEED_MAX        4095u   /* counts/sec (0 = max speed) */

/* Timeout for servo response (ms) */
#define SB_RX_TIMEOUT_MS    5u

/*
 * servo_bus_init() - configure USART3 in half-duplex mode (PB10, AF7) at
 * SB_BAUD (1 Mbps, 8N1). Call once at startup before gimbal_init().
 */
void servo_bus_init(void);

/*
 * servo_bus_write_pos(id, raw_pos, speed) - write goal position and moving
 * speed in a single WRITE DATA packet. raw_pos: 0-4095. speed: 0=max, 1-4095.
 * Returns true on successful TX (no response expected on write).
 */
bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed);

/*
 * servo_bus_write_torque(id, enable) - enable or disable servo torque.
 */
bool servo_bus_write_torque(uint8_t id, bool enable);

/*
 * servo_bus_read_pos(id, raw_pos) - read present position.
 * Returns true on success; raw_pos is 0-4095.
 */
bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos);

/*
 * servo_bus_read_speed(id, speed) - read present speed.
 * speed bit 15 is direction. Returns magnitude in lower 15 bits.
 */
bool servo_bus_read_speed(uint8_t id, uint16_t *speed);

/*
 * servo_bus_deg_to_raw(deg) - convert degree (-180..+180) to raw position.
 * Center (0 deg) = 2048. Clamps to 0-4095.
 */
uint16_t servo_bus_deg_to_raw(float deg);

/*
 * servo_bus_raw_to_deg(raw) - convert raw position (0-4095) to degree (-180..+180).
 */
float servo_bus_raw_to_deg(uint16_t raw);

#endif /* SERVO_BUS_H */