#include "ServoManager.h"
#include "../config/ConfigManager.h"
#include "../external/imgui/imgui.h"
#include "../global.h"
#include "../LanguageManager.h"
#include "../SystemInfo.h"
//#include "../../external/imgui/implot.h"
//#include "../Logger.h"

ServoManager::ServoManager(PLCAxis* axis)
	: m_IsShowServo(false)
	, m_Axis(axis)
	, m_UpdateFlag(false)
	, m_UpdateThread(INVALID_HANDLE_VALUE)
	, m_IsLogReady(false)
	//, m_IsRun(false)
	//, m_AxisLog(NULL)
{
	InitializeCriticalSection(&m_LogLoadCS);
	InitializeCriticalSection(&m_ShowLoadCS);
}

ServoManager::~ServoManager()
{
	Shutdown();
	DeleteCriticalSection(&m_LogLoadCS);
	DeleteCriticalSection(&m_ShowLoadCS);
}

void ServoManager::Startup()
{
	if (m_UpdateThread != INVALID_HANDLE_VALUE)return;
	m_UpdateFlag = true;
	m_UpdateThread = AtlCreateThread(UpdateProc, this);
}

void ServoManager::Shutdown()
{
	m_UpdateFlag = false;
	if (m_UpdateThread != INVALID_HANDLE_VALUE)
	{
		if (WaitForSingleObject(m_UpdateThread, 500)==WAIT_TIMEOUT) {
			TerminateThread(m_UpdateThread, 1);
		}
		CloseHandle(m_UpdateThread);
		m_UpdateThread = INVALID_HANDLE_VALUE;
	}
}


DWORD WINAPI ServoManager::UpdateProc(ServoManager* _this)
{
	if (_this) {
		_this->UpdateRun();
	}
	return 0;
}

void ServoManager::UpdateRun()
{

	while (m_UpdateFlag) {

		float moldTorque = m_Axis->m_Mold->GetState()->m_Torque->GetValue() / m_Axis->m_Mold->GetState()->m_AxisCfg->m_MaxLoadValue->GetValue() * 100.0f;
		float loadTorque = m_Axis->m_Load->GetState()->m_Torque->GetValue() / m_Axis->m_Load->GetState()->m_AxisCfg->m_MaxLoadValue->GetValue() * 100.0f;
		float armTorque = m_Axis->m_Arm->GetState()->m_Torque->GetValue() / m_Axis->m_Arm->GetState()->m_AxisCfg->m_MaxLoadValue->GetValue() * 100.0f;
		float supplyTorque = m_Axis->m_Supply->GetState()->m_Torque->GetValue() / m_Axis->m_Supply->GetState()->m_AxisCfg->m_MaxLoadValue->GetValue() * 100.0f;
		EnterCriticalSection(&m_LogLoadCS);
		if (m_IsLogReady) {
			m_MoldStat.logLoadList.push_back(moldTorque);
			m_LoadStat.logLoadList.push_back(loadTorque);
			m_ArmStat.logLoadList.push_back(armTorque);
			m_SupplyStat.logLoadList.push_back(supplyTorque);
		}
		
		LeaveCriticalSection(&m_LogLoadCS);

		EnterCriticalSection(&m_ShowLoadCS);
		while (m_MoldStat.showLoadList.size() >= ServoStat::MAX_SHOW_LOAD_SIZE) {
			m_MoldStat.showLoadList.pop_front();
		}
		m_MoldStat.showLoadList.push_back(moldTorque);

		while (m_LoadStat.showLoadList.size() >= ServoStat::MAX_SHOW_LOAD_SIZE) {
			m_LoadStat.showLoadList.pop_front();
		}
		m_LoadStat.showLoadList.push_back(loadTorque);

		while (m_ArmStat.showLoadList.size() >= ServoStat::MAX_SHOW_LOAD_SIZE) {
			m_ArmStat.showLoadList.pop_front();
		}
		m_ArmStat.showLoadList.push_back(armTorque);

		while (m_SupplyStat.showLoadList.size() >= ServoStat::MAX_SHOW_LOAD_SIZE) {
			m_SupplyStat.showLoadList.pop_front();
		}
		m_SupplyStat.showLoadList.push_back(supplyTorque);
		LeaveCriticalSection(&m_ShowLoadCS);

		Sleep(100);
	}
}

void ServoManager::StartLog(AxisLog* al)
{
	EnterCriticalSection(&m_LogLoadCS);
	m_MoldStat.logLoadList.clear();
	m_LoadStat.logLoadList.clear();
	m_ArmStat.logLoadList.clear();
	m_SupplyStat.logLoadList.clear();
	m_IsLogReady = true;
	LeaveCriticalSection(&m_LogLoadCS);
	if (al) {
		al->m_PreArmPos = m_Axis->m_Arm->GetState()->GetShowPos();
		g_SystemInfo->LockInfo();
		al->m_PreLinearEncoderPos = g_SystemInfo->m_EnvState.m_LinearActDistance;
		g_SystemInfo->UnlockInfo();
		al->m_PreMoldPos = m_Axis->m_Mold->GetState()->GetShowPos() / 1000.0f;
		al->m_PreLoadPos = m_Axis->m_Load->GetState()->GetShowPos() / 1000.0f;
		al->m_PreSupplyPos = m_Axis->m_Supply->GetState()->GetShowPos() / 1000.0f;
	}
}

void ServoManager::StopLog(AxisLog* lc)
{
	EnterCriticalSection(&m_LogLoadCS);
	m_IsLogReady = false;
	if (!m_MoldStat.logLoadList.empty()) {
		lc->m_MoldDataCount =(uint32_t) m_MoldStat.logLoadList.size();
		lc->m_MoldData = new float[lc->m_MoldDataCount];
	}
	uint32_t moldFlag = 0;
	for (list<float>::iterator it = m_MoldStat.logLoadList.begin(); it != m_MoldStat.logLoadList.end(); it++) {
		if (moldFlag < lc->m_MoldDataCount)lc->m_MoldData[moldFlag++] = (*it);
	}

	if (!m_LoadStat.logLoadList.empty()) {
		lc->m_LoadDataCount = (uint32_t)m_LoadStat.logLoadList.size();
		lc->m_LoadData = new float[lc->m_LoadDataCount];
	}
	int powderFlag = 0;
	for (list<float>::iterator it = m_LoadStat.logLoadList.begin(); it != m_LoadStat.logLoadList.end(); it++) {
		if (powderFlag < lc->m_LoadDataCount)lc->m_LoadData[powderFlag++] = (*it);
	}

	if (!m_ArmStat.logLoadList.empty()) {
		lc->m_ArmDataCount = (uint32_t)m_ArmStat.logLoadList.size();
		lc->m_ArmData = new float[lc->m_ArmDataCount];
	}
	uint32_t  armFlag = 0;
	for (list<float>::iterator it = m_ArmStat.logLoadList.begin(); it != m_ArmStat.logLoadList.end(); it++) {
		if (armFlag < lc->m_ArmDataCount)lc->m_ArmData[armFlag++] = (*it);
	}

	if (!m_SupplyStat.logLoadList.empty()) {
		lc->m_SupplyDataCount = m_SupplyStat.logLoadList.size();
		lc->m_SupplyData = new float[lc->m_SupplyDataCount];
	}
	uint32_t supplyFlag = 0;
	for (list<float>::iterator it = m_SupplyStat.logLoadList.begin(); it != m_SupplyStat.logLoadList.end(); it++) {
		if (supplyFlag < lc->m_SupplyDataCount)lc->m_SupplyData[supplyFlag++] = (*it);
	}
	LeaveCriticalSection(&m_LogLoadCS);
	lc->m_AftArmPos = m_Axis->m_Arm->GetState()->GetShowPos();
	g_SystemInfo->LockInfo();
	lc->m_AftLinearEncoderPos = g_SystemInfo->m_EnvState.m_LinearActDistance;
	g_SystemInfo->UnlockInfo();
	lc->m_AftMoldPos = m_Axis->m_Mold->GetState()->GetShowPos() / 1000.0f;
	lc->m_AftLoadPos = m_Axis->m_Load->GetState()->GetShowPos() / 1000.0f;
	lc->m_AftSupplyPos = m_Axis->m_Supply->GetState()->GetShowPos() / 1000.0f;
}

void ServoManager::ResetLogServo()
{
	EnterCriticalSection(&m_LogLoadCS);
	m_MoldStat.logLoadList.clear();
	m_LoadStat.logLoadList.clear();
	m_ArmStat.logLoadList.clear();
	m_SupplyStat.logLoadList.clear();
	m_IsLogReady = false;
	LeaveCriticalSection(&m_LogLoadCS);
}

//void ServoManager::DrawServo()
//{
//	if (!m_IsShowServo)return;
//	ImGui::Begin(_(u8"扭力状态").c_str(), &m_IsShowServo, ImGuiWindowFlags_NoDocking);
//	vector<double> moldLoad;
//	vector<double> loadLoad;
//	vector<double> armLoad;
//	vector<double> supplyLoad;
//
//	double maxValue = 110.0;
//
//	EnterCriticalSection(&m_ShowLoadCS);
//	for (list<float>::iterator it = m_MoldStat.showLoadList.begin(); it != m_MoldStat.showLoadList.end(); it++) {
//		moldLoad.push_back((*it));
//	}
//
//	for (list<float>::iterator it = m_LoadStat.showLoadList.begin(); it != m_LoadStat.showLoadList.end(); it++) {
//		loadLoad.push_back(*it);
//	}
//
//	for (list<float>::iterator it = m_ArmStat.showLoadList.begin(); it != m_ArmStat.showLoadList.end(); it++) {
//		armLoad.push_back(*it);
//	}
//
//	for (list<float>::iterator it = m_SupplyStat.showLoadList.begin(); it != m_SupplyStat.showLoadList.end(); it++) {
//		supplyLoad.push_back(*it);
//	}
//	LeaveCriticalSection(&m_ShowLoadCS);
//
//	float cHeight = (ImGui::GetContentRegionAvail().y - 30.0f) / 4.0f;
//	float cWidth = ImGui::GetContentRegionAvail().x - 50;
//	ImGui::BeginChild("moldtorque", ImVec2(-1, cHeight), true);
//	ImGui::Text(_(u8"成型缸负载曲线").c_str());
//	if (ImPlot::BeginPlot("moldline", ImVec2(-1, -1), ImPlotFlags_NoLegend | ImPlotFlags_NoTitle)) {
//		ImPlot::SetupAxes(NULL, NULL, ImPlotAxisFlags_AutoFit | ImPlotAxisFlags_NoGridLines, ImPlotAxisFlags_AutoFit);
//		ImPlot::SetupAxisLimits(ImAxis_Y1, -10.0, maxValue, ImPlotCond_Always);
//		int ndata = (int)moldLoad.size();
//		if (ndata > 0) {
//
//			ImPlot::PlotLine("LinePlot", &moldLoad[0], ndata);
//			if (ImPlot::IsPlotHovered())
//			{
//				ImPlotPoint mouse = ImPlot::GetPlotMousePos();
//				int posX = round(mouse.x);
//				if (posX >= 0 && posX < ndata) {
//					ImGui::BeginTooltip();
//					ImGui::Text(_(u8"负载值:%.1f %%").c_str(), moldLoad[posX]);
//					ImGui::EndTooltip();
//				}
//			}
//		}
//		ImPlot::EndPlot();
//	}
//
//	ImGui::EndChild();
//
//
//	ImGui::BeginChild("loadtorque", ImVec2(-1, cHeight), true);
//	ImGui::Text(_(u8"移栽轴负载曲线").c_str());
//	if (ImPlot::BeginPlot("loadline", ImVec2(-1, -1), ImPlotFlags_NoLegend | ImPlotFlags_NoTitle)) {
//		ImPlot::SetupAxes(NULL, NULL, ImPlotAxisFlags_AutoFit | ImPlotAxisFlags_NoGridLines, ImPlotAxisFlags_AutoFit);
//		ImPlot::SetupAxisLimits(ImAxis_Y1, -10.0, maxValue, ImPlotCond_Always);
//		int ndata = (int)loadLoad.size();
//		if (ndata > 0) {
//
//			ImPlot::PlotLine("LinePlot", &loadLoad[0], ndata);
//			if (ImPlot::IsPlotHovered())
//			{
//				ImPlotPoint mouse = ImPlot::GetPlotMousePos();
//				int posX = round(mouse.x);
//				if (posX >= 0 && posX < ndata) {
//					ImGui::BeginTooltip();
//					ImGui::Text(_(u8"负载值:%.1f %%").c_str(), loadLoad[posX]);
//					ImGui::EndTooltip();
//				}
//			}
//		}
//		ImPlot::EndPlot();
//	}
//
//	ImGui::EndChild();
//
//	ImGui::BeginChild("armtorque", ImVec2(-1, cHeight), true);
//	ImGui::Text(_(u8"铺粉轴负载曲线").c_str());
//	if (ImPlot::BeginPlot("armline", ImVec2(-1, -1), ImPlotFlags_NoLegend | ImPlotFlags_NoTitle)) {
//		ImPlot::SetupAxes(NULL, NULL, ImPlotAxisFlags_AutoFit | ImPlotAxisFlags_NoGridLines, ImPlotAxisFlags_AutoFit);
//		ImPlot::SetupAxisLimits(ImAxis_Y1, -10.0, maxValue, ImPlotCond_Always);
//		int ndata = (int)armLoad.size();
//		if (ndata > 0) {
//
//			ImPlot::PlotLine("LinePlot", &armLoad[0], ndata);
//			if (ImPlot::IsPlotHovered())
//			{
//				ImPlotPoint mouse = ImPlot::GetPlotMousePos();
//				int posX = round(mouse.x);
//				if (posX >= 0 && posX < ndata) {
//					ImGui::BeginTooltip();
//					ImGui::Text(_(u8"负载值:%.1f %%").c_str(), armLoad[posX]);
//					ImGui::EndTooltip();
//				}
//			}
//		}
//		ImPlot::EndPlot();
//	}
//	ImGui::EndChild();
//
//	
//	ImGui::BeginChild("supplytorque", ImVec2(-1, cHeight), true);
//	ImGui::Text(_(u8"下粉轴负载曲线").c_str());
//	if (ImPlot::BeginPlot("supplyline", ImVec2(-1, -1), ImPlotFlags_NoLegend | ImPlotFlags_NoTitle)) {
//		ImPlot::SetupAxes(NULL, NULL, ImPlotAxisFlags_AutoFit | ImPlotAxisFlags_NoGridLines, ImPlotAxisFlags_AutoFit);
//		ImPlot::SetupAxisLimits(ImAxis_Y1, -10.0, maxValue, ImPlotCond_Always);
//		int ndata = (int)supplyLoad.size();
//		if (ndata > 0) {
//
//			ImPlot::PlotLine("LinePlot", &supplyLoad[0], ndata);
//			if (ImPlot::IsPlotHovered())
//			{
//				ImPlotPoint mouse = ImPlot::GetPlotMousePos();
//				int posX = round(mouse.x);
//				if (posX >= 0 && posX < ndata) {
//					ImGui::BeginTooltip();
//					ImGui::Text(_(u8"负载值:%.1f %%").c_str(), supplyLoad[posX]);
//					ImGui::EndTooltip();
//				}
//			}
//		}
//		ImPlot::EndPlot();
//	}
//	
//	ImGui::EndChild();
//
//	ImGui::End();
//}