#ifndef USER_CTRE_H_
#define USER_CTRE_H_

#include "ch32v20x.h"


// Control frames
#define CTRE_ENABLE				0x000401BF
#define CTRE_STATUS(n)			(0x00041400 + 0x40 * (n - 1))  // 1-15 valid
#define CTRE_CONTROL(n)			(0x00040000 + 0x40 * (n - 1))  // 1-6 valid

// Parameters
#define CTRE_PARAM_REQ			0x00001800
#define CTRE_PARAM_RESP			0x00001840
#define CTRE_PARAM_SET			0x00001880

#define CTRE_DEVICE_TYPE_MASK	0xFFFF0000
#define CTRE_TALON_SRX			0x02040000
#define CTRE_VICTOR_SPX			0x01040000

// Motorcontroller
#define CTRE_FACTORY_RESET		0x0000F900

// Broadcast port messages
#define CTRE_RESET_PORTS		0x0004F9BF
#define CTRE_GET_ALL_PORTS		0x0004F9FF


extern int ctre_port;

void ctre_init();
void ctre_feed_enable(u8 enable);
void ctre_percent_out(u8 id, double cmd);
void ctre_position_out(u8 id, int pos);
void ctre_set_param(u8 id, u16 param_idx, u32 value, u8 sub_value, u32 ordinal);
void ctre_factory_reset(u8 port);
void ctre_port_reset_all();

#endif /* USER_CTRE_H_ */