#include "motor.h"
#include "oled.h"
#include "pid.h"

void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4) {
    int16_t sp1, sp2, sp3, sp4;
    uint16_t tx_id; //can_id �tait d�clar� l� mais pas utilis�
    sp1 = sp2 = sp3 = sp4 = 0;
    tx_id = id;

    if (motor1) { sp1 = correct_output(motor1); }
    if (motor2) { sp2 = correct_output(motor2); }
    if (motor3) { sp3 = correct_output(motor3); }
    if (motor4) { sp4 = correct_output(motor4); }
		
		can1_transmit(tx_id, sp1, sp2, sp3, sp4);
}


static int16_t correct_output(motor_t *motor) {
    return motor->voltage;
}


static void get_6020_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
	motor->info.GM6020_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
	motor->info.GM6020_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
	motor->info.GM6020_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
	motor->info.GM6020_info.temp = (int8_t)(motor_buf[6]);
}

static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
	motor->info.M3508_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
	motor->info.M3508_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
	motor->info.M3508_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
	motor->info.M3508_info.temp = (int8_t)(motor_buf[6]);
}

static void get_2006_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
	motor->info.M2006_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
	motor->info.M2006_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
	motor->info.M2006_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
	motor->info.M2006_info.temp = (int8_t)(motor_buf[6]);
}

void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb)
{
	motor->voltage = pid_calc(&pid_param, target, fdb);
}


uint8_t get_motor_data(motor_t *motor) {
    static uint8_t motor_buf[CAN_DATA_SIZE];
    can1_read(motor->can_info.rx_id, motor_buf);
		
	
    switch (motor->type) {
			case GM6020:
				get_6020_data(motor, motor_buf);
				return 1;
			case M3508:
				get_3508_data(motor, motor_buf);
				return 1;
			default:
				return 0;
    }
}


void set_motor_position(motor_t *motor)
{
	float target = motor->target;
	switch(motor->type){
		case M3508:
				run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
		case GM6020:
				run_motor_from_command(motor, motor->pid_pos, target, motor->info.GM6020_info.angle);
		/*default:
			bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
	}

}


void set_motor_speed(motor_t *motor)
{
	float target = motor->target;
	switch(motor->type){
		case M3508:
				run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);;
		case GM6020:
				run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);;
		/*default:
			bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
	}
}