#ifndef _MOTOR_H_
#define _MOTOR_H_

#include "bsp_can.h"
#include "bsp_pwm.h"
#include "pid.h"


#define CAN_TX1_ID 0x200
#define CAN_TX2_ID 0x1FF

#define CAN_RX1_START 0x201
#define CAN_RX2_START 0x205

enum motors {
	/* can motors */
	M3508,
	GM6020,
	M2006,
	general_motor,
	/* pwm motors */
	M2305,
};
//

typedef struct {    
    uint16_t    rx_id;
    uint16_t    tx_id;
    uint8_t     can_id;
}   can_info_t;



/**
 * @struct  GM6020_info_t
 * @brief   store GM6020 motor data
 * @var rx_id   sensor CAN receive id
 * @var tx_id   sensor CAN transmit id
 * @var can_id  CAN id chosen from [CAN1, CAN2]
 * @var angle       most recent angle data
 * @var current_get actual current / torque output
 * @var current_set target current / torque output
 */
typedef struct {    
    int16_t     angle;
    int16_t     speed;
    int16_t     current;
		int8_t			temp;
}   GM6020_info_t;

typedef struct {    
    int16_t     angle;
    int16_t     speed;
    int16_t     current;
		int8_t			temp;
}   M3508_info_t;

typedef struct {    
    int16_t     angle;
    int16_t     speed;
    int16_t     current;
		int8_t			temp;
}   M2006_info_t;

typedef union{
	GM6020_info_t GM6020_info;
	M3508_info_t M3508_info;
	M2006_info_t M2006_info;
} motor_info_t;


// Une struct par moteur
typedef struct
{
	can_info_t can_info;	// Can ID information
	//M3508_info_t info;
	motor_info_t info;		// Specific info about the motor
	int type; 		// Type du moteur, ex:GM6020
	pid_struct_t pid_pos;			// Controler
	pid_struct_t pid_speed;			// Controler
	float target;
	float voltage;
} motor_t;
	
//id:0 -> 0x200, 1 -> 0x1ff
void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4);

static int16_t correct_output(motor_t *motor);
uint8_t get_motor_data(motor_t *motor);
static void get_6020_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
static void get_3508_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
static void get_2006_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);

void set_motor_position(motor_t *motor);
void set_motor_speed(motor_t *motor);
void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb);

#endif