#include "ctre.h"


int ctre_port;

void ctre_init() {
	ctre_port = 6;
}

void ctre_feed_enable(u8 enable) {
	CanTxMsg TxMessage;
	TxMessage.ExtId = CTRE_ENABLE;
	TxMessage.IDE = CAN_Id_Extended;
	TxMessage.RTR = CAN_RTR_Data;
	TxMessage.DLC = 8;
	TxMessage.Data[0] = enable;
	TxMessage.Data[1] = 0x00;
	TxMessage.Data[2] = 0x00;
	TxMessage.Data[3] = 0x00;
	TxMessage.Data[4] = 0x00;
	TxMessage.Data[5] = 0x00;
	TxMessage.Data[6] = 0x00;
	TxMessage.Data[7] = 0x00;
    CAN_Transmit(CAN1, &TxMessage);
}

void ctre_percent_out(u8 id, double cmd) {
	int cmdi = cmd * 1023;
	if(cmdi > 1023)  cmdi =  1023;
	if(cmdi < -1023) cmdi = -1023;
	CanTxMsg TxMessage;
	TxMessage.ExtId = CTRE_TALON_SRX | CTRE_CONTROL(3) | id;
	TxMessage.IDE = CAN_Id_Extended;
	TxMessage.RTR = CAN_RTR_Data;
	TxMessage.DLC = 8;
	TxMessage.Data[0] = cmdi >> 16;
	TxMessage.Data[1] = cmdi >> 8;
	TxMessage.Data[2] = cmdi;
	TxMessage.Data[3] = 0x00;
	TxMessage.Data[4] = 0x00;
	TxMessage.Data[5] = 0x00;
	TxMessage.Data[6] = 0x00;
	TxMessage.Data[7] = 0x00;
    CAN_Transmit(CAN1, &TxMessage);
}

void ctre_position_out(u8 id, int pos) {
	CanTxMsg TxMessage;
	TxMessage.ExtId = CTRE_TALON_SRX | CTRE_CONTROL(3) | id;
	TxMessage.IDE = CAN_Id_Extended;
	TxMessage.RTR = CAN_RTR_Data;
	TxMessage.DLC = 8;
	TxMessage.Data[0] = pos >> 16;
	TxMessage.Data[1] = pos >> 8;
	TxMessage.Data[2] = pos;
	TxMessage.Data[3] = 0x00;
	TxMessage.Data[4] = 0x00;
	TxMessage.Data[5] = 0x01;  // Invert?
	TxMessage.Data[6] = 0x00;
	TxMessage.Data[7] = 0x80; // 80
    CAN_Transmit(CAN1, &TxMessage);
}

void ctre_set_param(u8 id, u16 param_idx, u32 value, u8 sub_value, u32 ordinal) {
	u8 param_idx_h8 = param_idx >> 4;
	u8 param_idx_l4 = param_idx & 0xF;

	CanTxMsg TxMessage;
	TxMessage.ExtId = CTRE_TALON_SRX | CTRE_PARAM_SET | id;
	TxMessage.IDE = CAN_Id_Extended;
	TxMessage.RTR = CAN_RTR_Data;
	TxMessage.DLC = 8;
	TxMessage.Data[0] = param_idx_h8;
	TxMessage.Data[1] = ordinal | (param_idx_l4 << 4);
	TxMessage.Data[2] = value >> 0x18;
	TxMessage.Data[3] = value >> 0x10;
	TxMessage.Data[4] = value >> 0x08;
	TxMessage.Data[5] = value;
	TxMessage.Data[6] = 0x00;
	TxMessage.Data[7] = sub_value;
	CAN_Transmit(CAN1, &TxMessage);
}

void ctre_factory_reset(u8 port) {
	CanTxMsg tx_msg;
	tx_msg.ExtId = CTRE_TALON_SRX | CTRE_FACTORY_RESET | port;
	tx_msg.IDE = CAN_Id_Extended;
	tx_msg.RTR = CAN_RTR_Data;
	tx_msg.DLC = 3;
	tx_msg.Data[0] = 0x02;
	tx_msg.Data[1] = 0xC5;
	tx_msg.Data[2] = 0x01;
    CAN_Transmit(CAN1, &tx_msg);
}

void ctre_port_reset_all() {
	CanTxMsg tx_msg;
	tx_msg.ExtId = CTRE_RESET_PORTS;
	tx_msg.IDE = CAN_Id_Extended;
	tx_msg.RTR = CAN_RTR_Data;
	tx_msg.DLC = 0;
    CAN_Transmit(CAN1, &tx_msg);
}