#include "AxisState.h"
#include "../Communication/S7Command.h"
#include "../config/ConfigManager.h"
#include "../Logger.h"
#include "../DataManage/DataHandle.h"

AxisState::AxisState()
	: m_ServoOn(new BoolData("ServoOn", u8"轴伺服ON_RW"))
	, m_ServoHomeIndexOn(new BoolData("ServoHomeIndexOn", u8"轴伺服成立原点_RW"))
	, m_ServoReset(new BoolData("ServoReset", u8"轴伺服复位_RW"))
	, m_MoveP(new BoolData("MoveP", u8"轴相对左运动_手动_RW"))
	, m_MoveN(new BoolData("MoveN", u8"轴相对右运动_手动_RW"))
	, m_MovePContinue(new BoolData("MovePContinue", u8"轴左JOG_手动_RW"))
	, m_MoveNContinue(new BoolData("MoveNContinue", u8"轴右JOG_手动_RW"))
	, m_MoveAbsPos(new BoolData("MoveAbsPos", u8"轴绝对值运动_手动_RW"))
	, m_MotionStop(new BoolData("MotionStop", u8"轴急停_RW"))
	, m_ServoRDY(new BoolData("ServoRDY", u8"轴伺服RDY状态_R"))
	, m_ServoHomeDone(new BoolData("ServoHomeDone", u8"轴指令运行HOME_Done_R"))
	, m_ResetDone(new BoolData("ResetDone", u8"轴指令运行RESET_Done_R"))
	, m_ServoBusy(new BoolData("ServoBusy", u8"轴伺服BUSY_R"))
	, m_ServoException(new BoolData("ServoException", u8"轴伺服异常_R"))
	, m_Runable(new BoolData("Runable", u8"轴可运行状态_R"))
	, m_ExceptionCode(new ShortData("ExceptionCode", u8"轴伺服异常字_R"))
	, m_Pos(new FloatData("Pos", u8"当前位置"))
	, m_Torque(new FloatData("Torque", u8"当前扭矩"))
	, m_ShowPos(new FloatData("ShowPos"))
	, m_RPos(new FloatData("RPos"))
{
	size_t ptrSize = sizeof(nullptr);   //指针大小
	void* startPtr = &m_startFlag + 1;
	size_t count = ((size_t)&m_endFlag - (size_t)startPtr) / ptrSize;
	InsertMp(startPtr, count);
}

AxisState::~AxisState() {
	std::shared_lock<std::shared_mutex> lock(mtx);
	for (auto item = m_baseMp.begin(); item != m_baseMp.end(); ++item) {
		delete item->second;
	}
	m_baseMp.clear();
}

void AxisState::GetValue(AxisStateValue& value)
{
	std::shared_lock<std::shared_mutex> lock(mtx);
	value.ServoOn = m_ServoOn->GetValue();
	value.ServoHomeIndexOn = m_ServoHomeIndexOn->GetValue();
	value.ServoReset = m_ServoReset->GetValue();
	value.MoveP = m_MoveP->GetValue();
	value.MoveN = m_MoveN->GetValue();
	value.MovePContinue = m_MovePContinue->GetValue();
	value.MoveNContinue = m_MoveNContinue->GetValue();
	value.MoveAbsPos = m_MoveAbsPos->GetValue();
	value.MotionStop = m_MotionStop->GetValue();
	value.ServoRDY = m_ServoRDY->GetValue();
	value.ServoHomeDone = m_ServoHomeDone->GetValue();
	value.ResetDone = m_ResetDone->GetValue();
	value.ServoException = m_ServoException->GetValue();
	value.Runable = m_Runable->GetValue();
	value.ExceptionCode = m_ExceptionCode->GetValue();
	value.Pos = m_Pos->GetValue();
	value.Torque = m_Torque->GetValue();
	value.ServoBusy = m_ServoBusy->GetValue();
	value.ShowPos = m_ShowPos->GetValue();
	value.RPos = m_RPos->GetValue();

}

//void AxisState::Update(unsigned char* addr)
//{
//	std::unique_lock<std::shared_mutex> lock(mtx);
//	m_ServoOn= ((addr[0] & 0x1)>0 ? true : false);
//	m_ServoHomeIndexOn= ((addr[0] & 0x2)>0 ? true : false);
//	m_ServoReset= ((addr[0] & 0x4)>0 ? true : false);
//	m_MoveP= ((addr[0] & 0x8)>0 ? true : false);
//	m_MoveN= ((addr[0] & 0x10)>0 ? true : false);
//	m_MovePContinue= ((addr[0] & 0x20)>0 ? true : false);
//	m_MoveNContinue= ((addr[0] & 0x40)>0 ? true : false);
//	m_MoveAbsPos= ((addr[0] & 0x80)>0 ? true : false);
//	m_MotionStop= ((addr[1] & 0x1)>0 ? true : false);
//	m_ServoRDY= ((addr[1] & 0x2)>0 ? true : false);
//	m_ServoHomeDone= ((addr[1] & 0x4)>0 ? true : false);
//	m_ResetDone= ((addr[1] & 0x8)>0 ? true : false);
//	m_ServoBusy= ((addr[1] & 0x10)>0 ? true : false);
//	m_ServoException= ((addr[1] & 0x20)>0 ? true : false);
//	m_Runable= ((addr[1] & 0x40)>0 ? true : false);
//	m_ExceptionCode = S7WORDDATA(addr[3], addr[2]).wValue;
//}

void AxisState::Update(const ReadData& rd)
{
	std::unique_lock<std::shared_mutex> lock(mtx);
	if (!rd.result) {
		printf("data is error...");
		return;
	}

	for (auto& it : rd.its) {
		if (m_baseMp.find(it.nameKey) == m_baseMp.end()) {
			printf("error, key %s do not find...\n", it.nameKey.c_str());
			continue;
		}
		if(it.valueType ==  DATATYPE::iBOOL){
			m_baseMp[it.nameKey]->SetValue((bool)stoi(it.strValue));
		}
		else if (it.valueType == DATATYPE::iINT) {
			m_baseMp[it.nameKey]->SetValue(stoi(it.strValue));
		}
		else if (it.valueType == DATATYPE::iSHORT) {
			m_baseMp[it.nameKey]->SetValue((short)stoi(it.strValue));
		}
		else if (it.valueType == DATATYPE::iFLOAT) {
			m_baseMp[it.nameKey]->SetValue(stof(it.strValue));
		}
		else {
			printf("error,type:%d do not find...\n", it.valueType);
		}
	}

}

//void AxisState::UpdatePosAndLoad(unsigned char* addr)
//{
//	int index = 0;
//	std::unique_lock<std::shared_mutex> lock(mtx);
//	m_Pos = (S7FLOATDATA(addr[index + 3], addr[index + 2], addr[index + 1], addr[index]).fValue); index += 4;
//	m_Torque = (S7FLOATDATA(addr[index + 3], addr[index + 2], addr[index + 1], addr[index]).fValue); index += 4;
//
//	if (!m_AxisCfg->m_ShowPosInv) {
//		m_ShowPos = m_Pos - m_AxisCfg->m_ShowRefZero;
//	}
//	else {
//		m_ShowPos = -(m_Pos - m_AxisCfg->m_ShowRefZero);
//	}
//
//	m_RPos = m_ShowPos / (m_AxisCfg->m_active_limit - m_AxisCfg->m_negactive_limit);
//}

float AxisState::GetShowPos()
{
	float showPos = 0.0f;
	std::unique_lock<std::shared_mutex> lock(mtx);
	showPos = m_ShowPos->GetValue();
	return showPos;
}

void AxisState::SetZeroPos()
{
	m_AxisCfg->m_ShowRefZero->SetValue(m_Pos->GetValue());
	//ConfigManager::Instance()->UpdateZeroOffset(m_AxisCfg->m_axis_id, m_AxisCfg->m_ShowRefZero);
	DataHandle::Instance()->SetPushMsg(SETZEROPOS, std::to_string(m_AxisCfg->m_axis_id),std::to_string(m_AxisCfg->m_ShowRefZero->GetValue()),UNKNOW);
}


void  AxisState::InsertMp(void* startPtr, size_t count, const string& suff) {
	size_t ptrSize = sizeof(nullptr);   //指针大小
	for (size_t i = 0; i < count; ++i) {
		BaseData* bd = *((BaseData**)((char*)startPtr + ptrSize * i));
		string key = bd->GetCode() + suff;
		if (m_baseMp.find(key) != m_baseMp.end()) {
			printf("%s is repeated...\n", key.data());
		}
		else { m_baseMp.insert(make_pair(key, bd)); }
	}
}


void MainAxisState::GetValue(AxisStateValue& value)
{
	std::shared_lock<std::shared_mutex> lock(mtx);
	value.ServoOn = m_ServoOn->GetValue();
	value.ServoHomeIndexOn = m_ServoHomeIndexOn->GetValue();
	value.ServoReset = m_ServoReset->GetValue();
	value.MoveP = m_MoveP->GetValue();
	value.MoveN = m_MoveN->GetValue();
	value.MovePContinue = m_MovePContinue->GetValue();
	value.MoveNContinue = m_MoveNContinue->GetValue();
	value.MoveAbsPos = m_MoveAbsPos;
	value.MotionStop = m_MotionStop->GetValue();
	value.ServoRDY = m_ServoRDY->GetValue();
	value.ServoHomeDone = m_ServoHomeDone->GetValue();
	value.ResetDone = m_ResetDone->GetValue();
	value.ServoException = m_ServoException->GetValue();
	value.Runable = m_Runable->GetValue();
	value.ExceptionCode = m_ExceptionCode->GetValue();
	value.Pos = m_Pos->GetValue();
	value.Torque = m_Torque->GetValue();
	value.ShowPos = m_ShowPos->GetValue();
	value.RPos = m_RPos->GetValue();
	value.ServoBreakOn = m_ServoBreakOn->GetValue();
	value.BindSlaveOn = m_BindSlaveOn->GetValue();
	value.BindSlaveFinish = m_BindSlaveFinish->GetValue();
	value.ServoBusy = m_ServoBusy->GetValue();
}


//void MainAxisState::Update(unsigned char* addr)
//{
//	std::unique_lock<std::shared_mutex> lock(mtx);
//	m_ServoOn= ((addr[0] & 0x1)>0 ? true : false);
//	m_ServoHomeIndexOn= ((addr[0] & 0x2)>0 ? true : false);
//	m_ServoReset= ((addr[0] & 0x4)>0 ? true : false);
//	m_ServoBreakOn= ((addr[0] & 0x8)>0 ? true : false);
//	m_BindSlaveOn= ((addr[0] & 0x10)>0 ? true : false);
//	m_MoveP= ((addr[0] & 0x20)>0 ? true : false);
//	m_MoveN= ((addr[0] & 0x40)>0 ? true : false);
//	m_MovePContinue= ((addr[0] & 0x80)>0 ? true : false);
//	m_MoveNContinue= ((addr[1] & 0x1)>0 ? true : false);
//	m_MoveAbsPos= ((addr[1] & 0x2)>0 ? true : false);
//	m_MotionStop= ((addr[1] & 0x4)>0 ? true : false);
//	m_ServoRDY= ((addr[1] & 0x8)>0 ? true : false);
//	m_ServoHomeDone= ((addr[1] & 0x10)>0 ? true : false);
//	m_ResetDone= ((addr[1] & 0x20)>0 ? true : false);
//	bool lastServoBusy = m_ServoBusy;
//	m_ServoBusy= ((addr[1] & 0x40)>0 ? true : false);
////	if (lastServoBusy && !m_ServoBusy) {
////		g_log->TraceInfo("轴忙转换为：%d",m_ServoBusy?1:0);
//// }
//
//	m_ServoException= ((addr[1] & 0x80)>0 ? true : false);
//	m_BindSlaveFinish= ((addr[2] & 0x1)>0 ? true : false);
//	m_Runable= ((addr[2] & 0x2)>0 ? true : false);
//	m_ExceptionCode = S7WORDDATA(addr[5], addr[4]).wValue;
//}


void SlaveAxisState::GetValue(AxisStateValue& value)
{
	std::shared_lock<std::shared_mutex> lock(mtx);
	value.ServoOn = m_ServoOn->GetValue();
	value.ServoHomeIndexOn = m_ServoHomeIndexOn->GetValue();
	value.ServoReset = m_ServoReset->GetValue();
	value.MoveP = m_MoveP->GetValue();
	value.MoveN = m_MoveN->GetValue();
	value.MovePContinue = m_MovePContinue->GetValue();
	value.MoveNContinue = m_MoveNContinue->GetValue();
	value.MoveAbsPos = m_MoveAbsPos->GetValue();
	value.MotionStop = m_MotionStop->GetValue();
	value.ServoRDY = m_ServoRDY->GetValue();
	value.ServoHomeDone = m_ServoHomeDone->GetValue();
	value.ResetDone = m_ResetDone->GetValue();
	value.ServoException = m_ServoException->GetValue();
	value.Runable = m_Runable->GetValue();
	value.ExceptionCode = m_ExceptionCode->GetValue();
	value.Pos = m_Pos->GetValue();
	value.Torque = m_Torque->GetValue();
	value.ShowPos = m_ShowPos->GetValue();
	value.RPos = m_RPos->GetValue();
	value.SlaveServoBreakOn = m_ServoBreakOn->GetValue();
	value.SlaveUnbind = m_Unbind->GetValue();
	value.ServoBusy = m_ServoBusy->GetValue();
}

//void SlaveAxisState::Update(unsigned char* addr)
//{
//	std::unique_lock<std::shared_mutex> lock(mtx);
//	m_ServoOn= ((addr[0] & 0x1)>0 ? true : false);
//	m_ServoHomeIndexOn= ((addr[0] & 0x2)>0 ? true : false);
//	m_ServoReset= ((addr[0] & 0x4)>0 ? true : false);
//	m_ServoBreakOn= ((addr[0] & 0x8)>0 ? true : false);
//	m_Unbind= ((addr[0] & 0x10)>0 ? true : false);
//	m_MoveP= ((addr[0] & 0x20)>0 ? true : false);
//	m_MoveN= ((addr[0] & 0x40)>0 ? true : false);
//	m_MovePContinue= ((addr[0] & 0x80)>0 ? true : false);
//	m_MoveNContinue= ((addr[1] & 0x1)>0 ? true : false);
//	m_MoveAbsPos= ((addr[1] & 0x2)>0 ? true : false);
//	m_MotionStop= ((addr[1] & 0x4)>0 ? true : false);
//	m_ServoRDY= ((addr[1] & 0x8)>0 ? true : false);
//	m_ServoHomeDone= ((addr[1] & 0x10)>0 ? true : false);
//	m_ResetDone= ((addr[1] & 0x20)>0 ? true : false);
//	m_ServoBusy= ((addr[1] & 0x40)>0 ? true : false);
//	m_ServoException= ((addr[1] & 0x80)>0 ? true : false);
//	m_Runable= ((addr[2] & 0x1)>0 ? true : false);
//	m_ExceptionCode = S7WORDDATA(addr[5], addr[4]).wValue;
//}