#include "stdafx.h"
#include "SelfTestModel.h"
#include "SelfTestMotionStageArgs.h"
#include "MotionStages.h"
#include "IMachineryManager.h"
#include "ConfigurerMotion.h"
#include "IPositionManager.h"
#include "ITubeAngleController.h"
#include "ITubeHeightController.h"

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

SelfTestModel::SelfTestModel() 
:m_stageArgs(new SelfTestMotionStageArgs()),
m_coordinates(nullptr),
m_tubeAngle(nullptr),
m_tubeHeight(nullptr)
{
}


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

void SelfTestModel::ChangeStage(const std::string &stageName)
{
	m_StageName = stageName;
	OnMotionStage(stageName);
}

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

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

void SelfTestModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
{
	m_coordinates = coordinates;
	m_tubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
	m_tubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
}

void SelfTestModel::LoadMachineryParams(ResDataObject &params)
{

}

void SelfTestModel::LoadModelParams(ResDataObject &params)
{

}

void SelfTestModel::SetTechnicalParams(ResDataObject &params)
{

}

void SelfTestModel::OnFeedbackMotionParams(ResDataObject &params)
{

}

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

void SelfTestModel::OnMotionStage(const std::string &stagename)
{
	if (stagename == SELFTEST_STAGE_TUBE_HEIGHT_MOVETO_TARGET)
	{
		OnStageTubeHeightMoveToTarget();
	}
	else if (stagename == SELFTEST_STAGE_TUBE_HEIGHT_MOVE_BACK)
	{
		OnStageTubeHeightMoveBack();
	}
	else if (stagename == SELFTEST_STAGE_TUBE_ANGLE_ROTATETO_TARGET)
	{
		OnStageTubeAngleRotateToTarget();
	}
	else if (stagename == SELFTEST_STAGE_TUBE_ANGLE_ROTATE_BACK)
	{
		OnStageTubeAngleRotateBack();
	}
	else if (stagename == SELFTEST_STAGE_START_AUTO_SELFTEST)
	{
		OnStageAutoSelfTest();
	}
}

void SelfTestModel::OnStageAutoSelfTest()
{
	auto current = m_coordinates->GetCurrentPhysical((DOF_MECH)m_stageArgs->DOF);
	auto target = m_stageArgs->HighLimit;

	MoveMech(m_stageArgs->DOF, current, target, m_stageArgs->Period, m_stageArgs->DutyCycle);
}

void SelfTestModel::OnStageTubeHeightMoveToTarget()
{
	auto current = m_coordinates->GetCurrentPhysical((DOF_MECH)m_stageArgs->DOF);
	auto target = m_stageArgs->LowLimit;

	MoveMech(1, current, target, m_stageArgs->Period, m_stageArgs->DutyCycle);
}

void SelfTestModel::OnStageTubeHeightMoveBack()
{
	auto current = m_coordinates->GetCurrentPhysical((DOF_MECH)m_stageArgs->DOF);
	auto target = m_stageArgs->HighLimit;

	MoveMech(1, current, target, m_stageArgs->Period, m_stageArgs->DutyCycle);
}

void SelfTestModel::OnStageTubeAngleRotateToTarget()
{
	auto current = m_coordinates->GetCurrentPhysical((DOF_MECH)m_stageArgs->DOF);
	auto target = m_stageArgs->LowLimit;

	MoveMech(0, current, target, m_stageArgs->Period, m_stageArgs->DutyCycle);
}

void SelfTestModel::OnStageTubeAngleRotateBack()
{
	auto current = m_coordinates->GetCurrentPhysical((DOF_MECH)m_stageArgs->DOF);
	auto target = m_stageArgs->HighLimit;

	MoveMech(0, current, target, m_stageArgs->Period, m_stageArgs->DutyCycle);
}

void SelfTestModel::MoveMech(int dof, float current, float target, int period, float dutyCycle)
{
	if (dof == 0)
	{
		RotateTubeAngle(current, target, period,dutyCycle);
	}
	else if (dof == 1)
	{
		MoveTubeHeight(current, target, period, dutyCycle);
	}
}

void SelfTestModel::RotateTubeAngle(float current, float target, int period, float dutyCycle)
{
	float offset = abs(current - target);
	auto step = m_coordinates->ConvertMotorStepValue(m_tubeAngle->Name(), offset);
	auto direction = JudgeDirectionInTubeAngleAxis(current, target);
	m_tubeAngle->Rotate(
		direction,
		step,
		period,
		dutyCycle);
}

void SelfTestModel::MoveTubeHeight(float current, float target, int period, float dutyCycle)
{
	float offset = abs(current - target);

	auto step = m_coordinates->ConvertMotorStepValue(m_tubeHeight->Name(), offset);
	auto direction = JudgeDirectionInTubeHeightAxis(current, target);
	m_tubeHeight->Move(
		direction,
		step,
		period,
		dutyCycle
		);
}

int SelfTestModel::JudgeDirectionInTubeHeightAxis(float current, float target)
{
	auto positive = ConfigurerMotion::GetTubeHeightAxisPositiveDirection();
	auto direction = positive > 0 ? 1 : -1;

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

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

	return 0;
}

int SelfTestModel::JudgeDirectionInTubeAngleAxis(float current, float target)
{
	auto positive = ConfigurerMotion::GetTubeRotateAxisPositiveDirection();
	auto direction = positive > 0 ? 1 : -1;

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

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

	return 0;
}

void SelfTestModel::SwitchScanningComponents(int nSwitch)
{

}

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