#include "AxisState.h"
#include "../Communication/S7Command.h"
#include "../config/ConfigManager.h"
#include "../Logger.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);
}


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->SetValue(((addr[0] & 0x1) > 0 ? true : false));
	m_ServoHomeIndexOn->SetValue(((addr[0] & 0x2) > 0 ? true : false));
	m_ServoReset->SetValue(((addr[0] & 0x4) > 0 ? true : false));
	m_MoveP->SetValue(((addr[0] & 0x8) > 0 ? true : false));
	m_MoveN->SetValue(((addr[0] & 0x10) > 0 ? true : false));
	m_MovePContinue->SetValue(((addr[0] & 0x20) > 0 ? true : false));
	m_MoveNContinue->SetValue(((addr[0] & 0x40) > 0 ? true : false));
	m_MoveAbsPos->SetValue(((addr[0] & 0x80) > 0 ? true : false));
	m_MotionStop->SetValue(((addr[1] & 0x1) > 0 ? true : false));
	m_ServoRDY->SetValue(((addr[1] & 0x2) > 0 ? true : false));
	m_ServoHomeDone->SetValue(((addr[1] & 0x4) > 0 ? true : false));
	m_ResetDone->SetValue(((addr[1] & 0x8) > 0 ? true : false));
	m_ServoBusy->SetValue(((addr[1] & 0x10) > 0 ? true : false));
	m_ServoException->SetValue(((addr[1] & 0x20) > 0 ? true : false));
	m_Runable->SetValue( ((addr[1] & 0x40)>0 ? true : false));
	m_ExceptionCode ->SetValue( S7WORDDATA(addr[3], addr[2]).wValue);
}

void AxisState::UpdatePosAndLoad(unsigned char* addr)
{
	int index = 0;
	std::unique_lock<std::shared_mutex> lock(mtx);
	m_Pos->SetValue((S7FLOATDATA(addr[index + 3], addr[index + 2], addr[index + 1], addr[index]).fValue)); index += 4;
	m_Torque->SetValue((S7FLOATDATA(addr[index + 3], addr[index + 2], addr[index + 1], addr[index]).fValue)); index += 4;

	if (!m_AxisCfg->m_ShowPosInv) {
		m_ShowPos->SetValue(m_Pos->GetValue() - m_AxisCfg->m_ShowRefZero->GetValue());
	}
	else {
		m_ShowPos->SetValue(-(m_Pos->GetValue() - m_AxisCfg->m_ShowRefZero->GetValue()));
	}

	m_RPos->SetValue(m_ShowPos->GetValue() / (m_AxisCfg->m_active_limit - m_AxisCfg->m_negactive_limit));
}

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

void AxisState::SetZeroPos()
{
	m_AxisCfg->m_ShowRefZero ->SetValue(m_Pos->GetValue());
	ConfigManager::GetInstance()->UpdateZeroOffset(m_AxisCfg->m_axis_id, (long)m_AxisCfg->m_ShowRefZero->GetValue());
}

//void AxisState::SendToClients() {
//	/*m_controller->m_Axis->m_Mold->GetState()->Update(msg);*/
//
//}
		

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->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.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->SetValue((addr[0] & 0x1) > 0 ? true : false);
	m_ServoHomeIndexOn->SetValue((addr[0] & 0x2) > 0 ? true : false);
	m_ServoReset->SetValue((addr[0] & 0x4) > 0 ? true : false);
	m_ServoBreakOn->SetValue((addr[0] & 0x8) > 0 ? true : false);
	m_BindSlaveOn->SetValue((addr[0] & 0x10) > 0 ? true : false);
	m_MoveP->SetValue((addr[0] & 0x20) > 0 ? true : false);
	m_MoveN->SetValue((addr[0] & 0x40) > 0 ? true : false);
	m_MovePContinue->SetValue((addr[0] & 0x80) > 0 ? true : false);
	m_MoveNContinue->SetValue((addr[1] & 0x1) > 0 ? true : false);
	m_MoveAbsPos->SetValue((addr[1] & 0x2) > 0 ? true : false);
	m_MotionStop->SetValue((addr[1] & 0x4) > 0 ? true : false);
	m_ServoRDY->SetValue((addr[1] & 0x8) > 0 ? true : false);
	m_ServoHomeDone->SetValue((addr[1] & 0x10) > 0 ? true : false);
	m_ResetDone->SetValue((addr[1] & 0x20) > 0 ? true : false);
	bool lastServoBusy = m_ServoBusy;
	m_ServoBusy->SetValue((addr[1] & 0x40) > 0 ? true : false);
//	if (lastServoBusy && !m_ServoBusy) {
//		g_log->TraceInfo("轴忙转换为：%d",m_ServoBusy?1:0);
// }

	m_ServoException->SetValue((addr[1] & 0x80) > 0 ? true : false);
	m_BindSlaveFinish->SetValue((addr[2] & 0x1) > 0 ? true : false);
	m_Runable->SetValue((addr[2] & 0x2) > 0 ? true : false);
	m_ExceptionCode->SetValue(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->SetValue((addr[0] & 0x1) > 0 ? true : false);
	m_ServoHomeIndexOn->SetValue((addr[0] & 0x2) > 0 ? true : false);
	m_ServoReset->SetValue((addr[0] & 0x4) > 0 ? true : false);
	m_ServoBreakOn->SetValue((addr[0] & 0x8) > 0 ? true : false);
	m_Unbind->SetValue((addr[0] & 0x10) > 0 ? true : false);
	m_MoveP->SetValue((addr[0] & 0x20) > 0 ? true : false);
	m_MoveN->SetValue((addr[0] & 0x40) > 0 ? true : false);
	m_MovePContinue->SetValue((addr[0] & 0x80) > 0 ? true : false);
	m_MoveNContinue->SetValue((addr[1] & 0x1) > 0 ? true : false);
	m_MoveAbsPos->SetValue((addr[1] & 0x2) > 0 ? true : false);
	m_MotionStop->SetValue((addr[1] & 0x4) > 0 ? true : false);
	m_ServoRDY->SetValue((addr[1] & 0x8) > 0 ? true : false);
	m_ServoHomeDone->SetValue((addr[1] & 0x10) > 0 ? true : false);
	m_ResetDone->SetValue((addr[1] & 0x20) > 0 ? true : false);
	m_ServoBusy->SetValue((addr[1] & 0x40) > 0 ? true : false);
	m_ServoException->SetValue((addr[1] & 0x80) > 0 ? true : false);
	m_Runable->SetValue((addr[2] & 0x1) > 0 ? true : false);
	m_ExceptionCode->SetValue(S7WORDDATA(addr[5], addr[4]).wValue);
}