//
// Created by huli on 2020/7/28.
//

#include "motor_manager.h"
Motor_manager::Motor_manager()
{
	m_motor_manager_status = MOTOR_MANAGER_UNKNOW;
	m_motor_number = 0;
}
Motor_manager::~Motor_manager()
{
	motor_manager_uninit();
}

//初始化 电机驱动器 的总管理 模块。如下三选一
Error_manager Motor_manager::motor_manager_init()
{
	return motor_manager_init_from_protobuf(MOTOR_PARAMETER_PATH);
}
//初始化 电机驱动器 的总管理模块。从文件读取
Error_manager Motor_manager::motor_manager_init_from_protobuf(std::string prototxt_path)
{
	Motor_manager_proto::Motor_manager_parameter_all t_motor_manager_parameter_all;

	if(!  proto_tool::read_proto_param(prototxt_path,t_motor_manager_parameter_all) )
	{
		return Error_manager(MOTOR_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Laser_manager read_proto_param  failed");
	}

	return motor_manager_init_from_protobuf(t_motor_manager_parameter_all);
}
//初始化 电机驱动器 的总管理 模块。从protobuf读取
Error_manager Motor_manager::motor_manager_init_from_protobuf(Motor_manager_proto::Motor_manager_parameter_all& motor_manager_parameter_all)
{
	LOG(INFO) << " --- motor_manager_init ---  "<< this;
	Error_manager t_error;

	int count = motor_manager_parameter_all.motor_manager_parameters_size();

	for (int i = 0; i < count; ++i)
	{

		int t_motor_device_id = motor_manager_parameter_all.motor_manager_parameters(i).motor_device_id();

		//查重
		if ( m_motor_driver_map.find(t_motor_device_id) == m_motor_driver_map.end() )
		{
			Motor_driver * tp_motor_driver = new Motor_driver;
			tp_motor_driver->motor_driver_init(t_motor_device_id,
											   motor_manager_parameter_all.motor_manager_parameters(i).can_device_id(),
											   motor_manager_parameter_all.motor_manager_parameters(i).can_port_id() );
			//添加到map
			m_motor_driver_map[t_motor_device_id] = tp_motor_driver;
		}
		else
		{
			LOG(INFO) << " Motor_manager::motor_manager_init_from_protobuf  motor_device_id repeat "<< this;
			//把后者忽略掉, 提示一级故障
			t_error.error_manager_reset(MOTOR_MANAGER_INDEX_REPEAT, NEGLIGIBLE_ERROR,
			"Motor_manager:: motor_device_id repeat");
		}
	}
	m_motor_manager_status = MOTOR_MANAGER_READY;
	m_motor_number = m_motor_driver_map.size();
	return t_error;
}
//反初始化
Error_manager Motor_manager::motor_manager_uninit()
{
	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end();  )
	{
		delete(iter->second);
		iter = m_motor_driver_map.erase(iter);
		//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
		//for循环不用 ++iter
	}

	m_motor_manager_status = MOTOR_MANAGER_UNKNOW;
	m_motor_number = 0;

	return Error_code::SUCCESS;
}

//检查总状态
Error_manager Motor_manager::check_status()
{
	if ( m_motor_manager_status == MOTOR_MANAGER_READY )
	{
	    return Error_code::SUCCESS;
	}
	else
	{
	    return Error_manager(Error_code::MOTOR_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
	    					" Motor_manager::check_status() error ");
	}
}
//检查消息是否有效, 判断这条消息是不是给我的.
Error_manager Motor_manager::check_msg(Can_message*  p_msg)
{
	//只要有一个检查成功, 那么就返回成功
	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end(); ++iter)
	{
		//每个驱动器只会执行自己的消息, 不是自己的不会执行
		if ( iter->second->check_msg(p_msg) == SUCCESS	)
		{
			return Error_code::SUCCESS;
		}
	}
	return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
						 " INVALID_MESSAGE error ");
}
//处理消息
Error_manager Motor_manager::execute_msg(Can_message* p_msg)
{
	//只要有一个处理成功, 那么就返回成功
	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end(); ++iter)
	{
		//每个驱动器只会执行自己的消息, 不是自己的不会执行
		if ( iter->second->execute_msg(p_msg) == SUCCESS	)
		{
			return Error_code::SUCCESS;
		}
	}
	return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
						 " INVALID_MESSAGE error ");
}

//定时封装发送消息,
Error_manager Motor_manager::encapsulate_send_data()
{
	Error_manager t_error;
	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end(); ++iter)
	{
		//定时封装发送消息,
		//必须执行完所有的子驱动器设备, 并将错误汇总
		t_error.compare_and_cover_error(iter->second->encapsulate_send_data());
	}
	return t_error;
}

Error_manager		Motor_manager::set_target_velocity(int motor_driver_id, float target_velocity)
{
	// std::cout<<"set vel"<<std::endl;
	return m_motor_driver_map[motor_driver_id]->set_target_velocity(target_velocity);
}
float 				Motor_manager::get_actual_velocity(int motor_driver_id)
{
	// std::cout<<"get vel"<<std::endl;
	return m_motor_driver_map[motor_driver_id]->get_actual_velocity();
}
//复位
Error_manager 		Motor_manager::motor_driver_reset(int motor_driver_id)
{
	return m_motor_driver_map[motor_driver_id]->motor_driver_reset();
}

//急停
Error_manager 		Motor_manager::motor_driver_stop(int motor_driver_id)
{
	return m_motor_driver_map[motor_driver_id]->motor_driver_stop();
}