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

AxisState::AxisState()
{
	
}

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

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::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;
	return showPos;
}

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

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


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;
	value.ServoHomeIndexOn = m_ServoHomeIndexOn;
	value.ServoReset = m_ServoReset;
	value.MoveP = m_MoveP;
	value.MoveN = m_MoveN;
	value.MovePContinue = m_MovePContinue;
	value.MoveNContinue = m_MoveNContinue;
	value.MoveAbsPos = m_MoveAbsPos;
	value.MotionStop = m_MotionStop;
	value.ServoRDY = m_ServoRDY;
	value.ServoHomeDone = m_ServoHomeDone;
	value.ResetDone = m_ResetDone;
	value.ServoException = m_ServoException;
	value.Runable = m_Runable;
	value.ExceptionCode = m_ExceptionCode;
	value.Pos = m_Pos;
	value.Torque = m_Torque;
	value.ShowPos = m_ShowPos;
	value.RPos = m_RPos;
	value.SlaveServoBreakOn = m_ServoBreakOn;
	value.SlaveUnbind = m_Unbind;
	value.ServoBusy = m_ServoBusy;
}

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;
}