#include "motor.h"
#include "oled.h"
#include "list_motor.h"

motor_t gm6020_up;
motor_t gm6020_down;
motor_t m3508_frontleft;
motor_t m3508_frontright;
motor_t m3508_backleft;
motor_t m3508_backright;
motor_t m2006;


const pid_struct_t M3508_pid_speed = { //A VERIFIER
		1, 2, 0.005, 2, 16000, 16000
	};
const pid_struct_t M3508_pid_pos = { //A VERIFIER
		0, 0.1, 0.005, 0, 16000, 16000
	};

const pid_struct_t GM6020_pid_speed = { //A VERIFIER
		1, 40, 10, 0, 30000, 30000
	};
const pid_struct_t GM6020_pid_pos = { //A VERIFIER
		0, 12, 3, 5, 10000, 10000
	};

const pid_struct_t M2006_pid_speed = { //A FAIRE
		1, 1.5, 0.1, 0, 16384, 16384
	};

const pid_struct_t M2006_pid_pos = { //A FAIRE
		0, 12, 3, 5, 10000, 10000
	};

//void motors_all_init(void){
//	
//	m3508_frontleft.type = M3508;
//	m3508_frontleft.can_info.can_id = 0;
//	m3508_frontleft.can_info.tx_id = 0;
//	m3508_frontleft.can_info.rx_id = 0x201; 
//	m3508_frontleft.pid_pos = M3508_pid_pos;
//	m3508_frontleft.pid_speed = M3508_pid_speed;
//	
//	m3508_frontright.type = M3508;
//	m3508_frontright.can_info.can_id = 0;
//	m3508_frontright.can_info.tx_id = 0;
//	m3508_frontright.can_info.rx_id = 0x202; 
//	m3508_frontright.pid_pos = M3508_pid_pos;
//	m3508_frontright.pid_speed = M3508_pid_speed;
//	
//	m3508_backleft.type = M3508;
//	m3508_backleft.can_info.can_id = 0;
//	m3508_backleft.can_info.tx_id = 0;
//	m3508_backleft.can_info.rx_id = 0x203; 
//	m3508_backleft.pid_pos = M3508_pid_pos;
//	m3508_backleft.pid_speed = M3508_pid_speed;
//	
//	m3508_backright.type = M3508;
//	m3508_backright.can_info.can_id = 0;
//	m3508_backright.can_info.tx_id = 0;
//	m3508_backright.can_info.rx_id = 0x204; 
//	m3508_backright.pid_pos = M3508_pid_pos;
//	m3508_backright.pid_speed = M3508_pid_speed;
//	
//	m2006.type = M2006;
//	m2006.can_info.can_id = 0;
//	m2006.can_info.tx_id = 1;
//	m2006.can_info.rx_id = 0x207; 
//	m2006.pid_pos = M2006_pid_pos;
//	m2006.pid_speed = M2006_pid_speed;

//	gm6020_up.type = GM6020;
//	gm6020_up.can_info.can_id = 0;
//	gm6020_up.can_info.tx_id = 1;
//	gm6020_up.can_info.rx_id = 0x205; 
//	gm6020_up.pid_pos = GM6020_pid_pos;
//	gm6020_up.pid_speed = GM6020_pid_speed;
//	gm6020_up.MIN_POSITION = 5565;
//	gm6020_up.MAX_POSITION = 6675;
//	
//	gm6020_down.type = GM6020;
//	gm6020_down.can_info.can_id = 0;
//	gm6020_down.can_info.tx_id = 1;
//	gm6020_down.can_info.rx_id = 0x206; 
//	gm6020_down.pid_pos = GM6020_pid_pos;
//	gm6020_down.pid_speed = GM6020_pid_speed;
//	gm6020_down.MIN_POSITION = 2519;
//	gm6020_down.MAX_POSITION = 7015;
//	
//	
//	gm6020_down.target = 4750; 
//	gm6020_up.target = 6200; 
//}


void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
	
	switch (mtr){
		case GM6020:
			motor->type = GM6020 ;
			motor->pid_pos = GM6020_pid_pos;
			motor->pid_speed = GM6020_pid_speed;
			motor->can_info.tx_id = 1;
			break;
		case M3508:
			motor->type = M3508 ;
			motor->pid_pos = M3508_pid_pos;
			motor->pid_speed = M3508_pid_speed;
			motor->can_info.tx_id = 0;
			break;
		case M2006:
			motor->type = M2006 ;
			motor->pid_pos = M2006_pid_pos;
			motor->pid_speed = M2006_pid_speed;
			motor->can_info.tx_id = 1;
			break;
		default :
			break;
	}
	motor->can_info.can_id = 0;
	motor->can_info.rx_id = motor_rx_id ;
}


void motors_all_init(void){
	motors_init(&m2006,  0x207, M2006);
	motors_init(&gm6020_down,  0x206, GM6020);
	motors_init(&gm6020_up,  0x205, GM6020);
	motors_init(&m3508_frontleft,  0x201, M3508);
	motors_init(&m3508_frontright,  0x202, M3508);
	motors_init(&m3508_backright,  0x203, M3508);
	motors_init(&m3508_backleft,  0x204, M3508);
	
	// Param�tres ICRA
//	gm6020_up.MIN_POSITION = 5565;
//	gm6020_up.MAX_POSITION = 6675;
//	gm6020_up.target = 6200;
//	gm6020_down.MIN_POSITION = 2519;
//	gm6020_down.MAX_POSITION = 7015;
//	gm6020_down.target = 4750; 
	
	// Param�tres StandardMeca
	gm6020_up.MIN_POSITION = 2220;
	gm6020_up.MAX_POSITION = 3840;
	gm6020_up.target = 3450;
	gm6020_down.MIN_POSITION = 2800;
	gm6020_down.MAX_POSITION = 6700;
	gm6020_down.target = 4750; 
}