#include "stdafx.h"
#include "CArmResetMotionModel.h"
#include "ResetMotionStageArgs.h"
#include "MotionStages.h"
#include "IMachineryManager.h"
#include "CArmComponentNameDef.h"
#include "ICircularController.h"
#include "ISwingController.h"
#include "ISensorEncoderController.h"
#include "ILandmarkController.h"
#include "IPositionManager.h"
#include "FluoroSwitchController.h"

using namespace DIOS::Dev::Detail::MachineryECOM;

CArmResetMotionModel::CArmResetMotionModel()
:m_StageName(""),
m_stageArgs(new ResetMotionStageArgs()),
m_coordinates(nullptr),
m_circular(nullptr),
m_swing(nullptr),
m_circularencoder(nullptr),
m_swingencoder(nullptr),
m_landmark(nullptr),
m_fluoroSwitch(nullptr),
m_resetSwingPeriod(250),
m_resetCircularPeriod(250),
m_tubeSwingLandmarkDirection(0),
m_tubeCircularLandmarkDirection(0),
m_tubeSwingResetAngle(0.0f),
m_tubeCircularResetAngle(0.0f),
m_tubeCircularAxisPositiveDirection(1),
m_tubeSwingAxisPositiveDirection(1)
{
}


CArmResetMotionModel::~CArmResetMotionModel()
{
	if (m_stageArgs)
	{
		delete m_stageArgs;
		m_stageArgs = nullptr;
	}
}

void CArmResetMotionModel::ChangeStage(const std::string &stageName)
{
	if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][ChangeStage]->[{$}]", stageName.c_str());

	m_StageName = stageName;
	OnMotionStage(m_StageName);
}

std::string CArmResetMotionModel::GetStageName()
{
	return m_StageName;
}

IMotionStageArgs *CArmResetMotionModel::GetStageArgs()
{
	return m_stageArgs;
}

void CArmResetMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
{
	m_coordinates = coordinates;

	m_circular = (ICircularController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR);
	m_swing = (ISwingController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING);
	m_circularencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER);
	m_swingencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING_ENCODER);
	m_landmark = (ILandmarkController *)(machineryManager->Resove(CONTROLLER_LANDMARK));
	m_fluoroSwitch = (IOutputController*)(machineryManager->Resove(CONTROLLER_FLUOROSWITCH));
}

void CArmResetMotionModel::LoadMachineryParams(ResDataObject &params)
{
	m_resetSwingPeriod = (DWORD)atoi((const char *)params["ResetRotatePeriod"]);
	m_resetCircularPeriod = (DWORD)atoi((const char *)params["ResetHeightPeriod"]);
	m_tubeSwingLandmarkDirection = (int)atoi((const char *)params["TubeRotateLandmarkDirection"]);
	m_tubeCircularLandmarkDirection = (int)atoi((const char *)params["TubeHeightLandmarkDirection"]);
	m_tubeSwingResetAngle = (float)atof((const char *)params["TubeRotateResetAngle"]);
	m_tubeCircularResetAngle = (float)atof((const char *)params["TubeHeightResetPos"]);
	m_tubeCircularAxisPositiveDirection = (int)atoi((const char *)params["TubeHeightAxisPositiveDirection"]);
	m_tubeSwingAxisPositiveDirection = (int)atoi((const char *)params["TubeRotateAxisPositiveDirection"]);
}

void CArmResetMotionModel::LoadModelParams(ResDataObject &params)
{

}

void CArmResetMotionModel::SetTechnicalParams(ResDataObject &params)
{

}

void CArmResetMotionModel::OnFeedbackMotionParams(ResDataObject &params)
{

}

BOOL CArmResetMotionModel::GetMotionParams(ResDataObject &params)
{
	return TRUE;
}

void CArmResetMotionModel::OnMotionStage(const std::string &stageName)
{
	if (stageName == RESET_STAGE_STOP_MOVE)
	{
		OnMotionStageStopMove();
	}
	else if (stageName == CARM_RESET_TUBE_SWING_FIND_LANDMARK)
	{
		OnMotionStageFindSwingLandmark();
	}
	else if (stageName == CARM_RESET_TUBE_CIRCULAR_FIND_LANDMARK)
	{
		OnMotionStageFindCircularLandmark();
	}
	else if (stageName == CARM_RESET_TUBE_SWING_ATTACH_Z_AXIS)
	{
		OnMotionStageAttachSwingZAxis();
	}
	else if (stageName == CARM_RESET_TUBE_SWING_DETACH_Z_AXIS)
	{
		OnMotionStageDetachSwingZAxis();
	}
	else if (stageName == CARM_RESET_TUBE_CIRCULAR_ATTACH_Z_AXIS)
	{
		OnMotionStageAttachCircularZAxis();
	}
	else if (stageName == CARM_RESET_TUBE_CIRCULAR_DETACH_Z_AXIS)
	{
		OnMotionStageDetachCircularZAxis();
	}
	else if (stageName == CARM_RESET_TUBE_SWING_FINISHRESET)
	{
		OnMotionStageSwingFinish();
	}
	else if (stageName == CARM_RESET_TUBE_CIRCULAR_FINISHRESET)
	{
		OnMotionStageCircularFinish();
	}
	else if (stageName == CARM_RESET_TUBE_SWING_STOP_AT_LANDMARK)
	{
		OnMotionStageSwingStopAtLandmark();
	}
	else if (stageName == CARM_RESET_TUBE_CIRCULAR_STOP_AT_LANDMARK)
	{
		OnMotionStageCircularStopAtLandmark();
	}

	else if (stageName == CARM_RESET_START_EXPOSURE)
	{
		OnStartExposure();
	}
	else if (stageName == CARM_RESET_STOP_EXPOSURE)
	{
		OnStopExposure();
	}
}

void CArmResetMotionModel::OnMotionStageStopMove()
{
	m_circular->StopRotation();
	m_swing->StopSwing();
}

void CArmResetMotionModel::OnMotionStageFindSwingLandmark()
{
	//if (IsTubeSwingAtResetLandmark())
	//{
	//	m_stageArgs->IsTubeAngleAtResetLandmark = TRUE;
	//	return;
	//}

	int direction = JudgeTubeSwingLandmarkDirection();
	m_swing->Swing(direction, -1, m_resetSwingPeriod);
}

BOOL CArmResetMotionModel::IsTubeSwingAtResetLandmark()
{
	BOOL ret = FALSE;

	if (m_tubeSwingLandmarkDirection > 0)
	{
		auto highstatus = m_landmark->ReadTubeAngleHighLandmarkStatus();
		ret = (highstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeAngleLandmark = 1;
		}
	}
	else
	{
		auto lowstatus = m_landmark->ReadTubeAngleLowLandmarkStatus();
		ret = (lowstatus == 1);
		if (ret)
		{
			m_stageArgs->ActivedTubeAngleLandmark = 0;
		}
	}

	return ret;
}

int  CArmResetMotionModel::JudgeTubeSwingLandmarkDirection()
{
	int direction = m_tubeSwingAxisPositiveDirection > 0 ? 1 : -1;

	return m_tubeSwingLandmarkDirection > 0 ? direction : (-1 * direction);
}

void CArmResetMotionModel::OnMotionStageFindCircularLandmark()
{
	
	//if (IsTubeCircularAtResetLandmark())
	//{
	//	if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnMotionStageFindCircularLandmark]->[Is At Landmark]");

	//	m_stageArgs->IsTubeHeightAtResetLandmark = TRUE;
	//	return;
	//}

	int direction = JudgeTubeCircularLandmarkDirection();

	if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnMotionStageFindCircularLandmark]->[dir {$:d}]", direction);

	m_circular->Rotate(direction, -1, m_resetCircularPeriod);
}

BOOL CArmResetMotionModel::IsTubeCircularAtResetLandmark()
{
	BOOL ret = FALSE;

	if (m_tubeCircularLandmarkDirection > 0)
	{
		auto highstatus = m_landmark->ReadTubeHeightHighLandmarkStatus();
		ret = (highstatus == 0/*1*/);
		if (ret)
		{
			m_stageArgs->ActivedTubeHeightLandmark = 1;
		}
	}
	else
	{
		auto lowstatus = m_landmark->ReadTubeHeightLowLandmarkStatus();
		ret = (lowstatus == 0/*1*/);
		if (ret)
		{
			m_stageArgs->ActivedTubeHeightLandmark = 0;
		}
	}

	return ret;
}

int  CArmResetMotionModel::JudgeTubeCircularLandmarkDirection()
{
	if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][JudgeTubeCircularLandmarkDirection]->[Positive:{$:d} Landmark:{$:d}]",
		m_tubeCircularAxisPositiveDirection, m_tubeCircularLandmarkDirection);

	int direction = m_tubeCircularAxisPositiveDirection > 0 ? 1 : -1;

	return m_tubeCircularLandmarkDirection > 0 ? direction : (-1 * direction);
}

void CArmResetMotionModel::OnMotionStageAttachSwingZAxis()
{
	m_swingencoder->BindABAxis();
	m_swingencoder->BindZAxis();
	m_swingencoder->ActiveAutoNotifyWhenZClear(true);
}

void CArmResetMotionModel::OnMotionStageDetachSwingZAxis()
{
	m_swingencoder->UnBindZAxis();
	m_swingencoder->ActiveAutoNotifyWhenZClear(false);
}

void CArmResetMotionModel::OnMotionStageAttachCircularZAxis()
{
	m_circularencoder->BindABAxis();
	m_circularencoder->BindZAxis();
	m_circularencoder->ActiveAutoNotifyWhenZClear(true);
}

void CArmResetMotionModel::OnMotionStageDetachCircularZAxis()
{
	m_circularencoder->UnBindZAxis();
	m_circularencoder->ActiveAutoNotifyWhenZClear(false);
}

void CArmResetMotionModel::OnMotionStageSwingFinish()
{
	auto resetAngle = GetTubeSwingResetPosition();
	auto tubeangle = m_coordinates->GetCurrentPhysical(CARM_TUBE_SWING);
	int direction = -1 * JudgeTubeSwingLandmarkDirection();

	if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnStageTubeSwingFinishReset]->[ResetPos:{$:f6} Current:{$:f6}]",resetAngle, tubeangle);

	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_SWING, abs(tubeangle - resetAngle));
	m_swing->Swing(direction, steps, m_resetSwingPeriod);
}

float CArmResetMotionModel::GetTubeSwingResetPosition()
{
	return m_tubeSwingResetAngle;
}

int   CArmResetMotionModel::JudgeSwingDirection(float current, float target)
{
	int direction = m_tubeSwingAxisPositiveDirection > 0 ? 1 : -1;

	if (current > target)
	{
		return -1 * direction;
	}

	if (current < target)
	{
		return 1 * direction;
	}

	return 0;
}

void CArmResetMotionModel::OnMotionStageCircularFinish()
{
	auto resetAngle = GetTubeCircularResetPosition();
	auto tubeangle = m_coordinates->GetCurrentPhysical(CARM_TUBE_CIRCULAR);
	int direction = -1 * JudgeTubeCircularLandmarkDirection();

	if (gmotionLog) gmotionLog->Info("[CArmResetMotionModel][OnStageTubeCircularFinishReset]->[ResetPos:{$:f6} Current:{$:f6}]", resetAngle, tubeangle);

	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_CIRCULAR, abs(tubeangle - resetAngle));
	m_circular->Rotate(direction, steps, m_resetCircularPeriod);
}

float CArmResetMotionModel::GetTubeCircularResetPosition()
{
	return m_tubeCircularResetAngle;
}

int  CArmResetMotionModel::JudgeCircularDirection(float current, float target)
{
	int direction = m_tubeCircularAxisPositiveDirection > 0 ? 1 : -1;

	if (current > target)
	{
		return -1 * direction;
	}

	if (current < target)
	{
		return 1 * direction;
	}

	return 0;
}

void CArmResetMotionModel::OnMotionStageSwingStopAtLandmark()
{
	m_swing->StopSwing();
}

void CArmResetMotionModel::OnMotionStageCircularStopAtLandmark()
{
	m_circular->StopRotation();
}

void CArmResetMotionModel::SwitchScanningComponents(int nSwitch)
{
	
}

void CArmResetMotionModel::OnStartExposure()
{
	if (m_fluoroSwitch)
	{
		if (gbusinessLog) gbusinessLog->Info("[CArmResetMotionModel][OnStartExposure]->[Enable fluoro switch signal]");
		m_fluoroSwitch->OutputSignal(true);
	}
}

void CArmResetMotionModel::OnStopExposure()
{
	if (m_fluoroSwitch)
	{
		if (gbusinessLog) gbusinessLog->Info("[CArmResetMotionModel][OnStopExposure]->[Disable fluoro switch signal]");
		m_fluoroSwitch->OutputSignal(false);
	}
}

void CArmResetMotionModel::SwitchWorkstation(string ws)
{
	m_CurWS = ws;
}