#include "stdafx.h"
#include "RADMotionModel.h"
#include "MotionStages.h"
#include "RADMotionStageArgs.h"
#include "IMachineryManager.h"
#include "ITubeAngleController.h"
#include "ITubeHeightController.h"
#include "ISensorADController.h"
#include "ISensorEncoderController.h"
#include "IPositionManager.h"
#include "DigitalTwinLogger.h"
#include "TubeHorizontalController.h"
#include "TubeLineMotionSwitchController.h"
#include "ConfigurerMotion.h"
#include "ConfigurerWS.h"

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

RADMotionModel::RADMotionModel() 
:m_StageName(""),
m_stageArgs(new RADMotionStageArgs()),
m_coordinates(nullptr),
m_motorTubeAngle(nullptr),
m_motorTubeHeight(nullptr),
m_adDetectorHeight(nullptr),
m_encoderTubeAngle(nullptr),
m_encoderTubeHeight(nullptr),
m_motorTubeHorizontal(nullptr),
m_adTubeHorizontal(nullptr),
m_encoderTubeHorizontal(nullptr),
m_tubeLineMotionSwitch(nullptr)
{
}


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

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

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

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

void RADMotionModel::OnMotionStage(const std::string &name)
{
	if (gmotionLog) gmotionLog->Info("[RADMotionModel][OnMotionStage]->[Enter][{$}]", name.c_str());

	if (name == RAD_STAGE_CLEAR_PARAMS)
	{
		OnStageClearParams();
	}
	else if (name == RAD_STAGE_STOP_MOVE)
	{
		OnStageStopMove();
	}
	else if (name == RAD_STAGE_CENTER_ADJUST)
	{
		OnStageCenterAdjust();
	}
	else if (name == RAD_STAGE_MOVE_TUBE_HEIGHT)
	{
		OnMotionStageMoveTubeHeight();
	}
	else if (name == RAD_STAGE_MOVE_TUBE_HORIZONTAL)
	{
		OnMotionStageMoveTubeHorizontal();
	}
	else if (name == RAD_STAGE_MOVE_TUBE_ROTATION)
	{
		OnMotionStageMoveTubeRotation();
	}
	else if (name == RAD_STAGE_ADJUST_SID_MOVE)
	{
		OnMotionStageMove2SID(m_CurWS);
	}

	if (gmotionLog) gmotionLog->Info("[RADMotionModel][OnMotionStage]->[Exit][{$}]", name.c_str());
}

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

	m_motorTubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
	m_motorTubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
	m_adDetectorHeight = (ISensorADController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD));
	m_encoderTubeAngle = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER));
	m_encoderTubeHeight = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER));

	m_motorTubeHorizontal = (ITubeHeightController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL));
	m_encoderTubeHorizontal = (ISensorEncoderController*)(machineryManager->Resove(CONTROLLER_TUBE_HORIZONTAL_ENCODER));
	m_tubeLineMotionSwitch = (IOutputController*)(machineryManager->Resove(CONTROLLER_TUBE_LINE_MOTION_SWTICH));

	assert(m_motorTubeAngle);
	assert(m_motorTubeHeight);
	assert(m_adDetectorHeight);
	assert(m_encoderTubeAngle);
	assert(m_encoderTubeHeight);
	assert(m_motorTubeHorizontal);
	assert(m_encoderTubeHorizontal);
}

void RADMotionModel::LoadMachineryParams(ResDataObject &params)
{
	m_MachineryParams = params;
}

void RADMotionModel::LoadModelParams(ResDataObject &params)
{
	
}

void RADMotionModel::SetTechnicalParams(ResDataObject &params)
{
	m_technicalParams = params;

	DigitalTwinLogger::Instance()->ReceiveExamMode("RAD");
}

void RADMotionModel::OnFeedbackMotionParams(ResDataObject &params)
{

}

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

void RADMotionModel::OnStageClearParams()
{
	m_motorTubeAngle->ClearSignal();
	m_encoderTubeHeight->ClearTrigger();
	m_encoderTubeHorizontal->ClearTrigger();
}

void RADMotionModel::OnStageStopMove()
{
	m_motorTubeAngle->StopRotation();
	m_motorTubeHeight->StopMove();
	m_motorTubeHorizontal->StopMove();

	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT);
	DigitalTwinLogger::Instance()->ReceiveTomoStartPosition(tubeAngleSensorValue, tubeHeightSensorValue);
	DigitalTwinLogger::Instance()->ReceiveMotionStopPosition(tubeAngleSensorValue, tubeHeightSensorValue);
	DigitalTwinLogger::Instance()->SaveMotionRecord();
}

void RADMotionModel::OnStageCenterAdjust()
{
	auto tubeAngleSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_ANGLE);
	auto tubeHeightSensorValue = m_coordinates->GetCurrentSensorValue(TOMO_TUBE_HEIGHT);
	auto detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
	DigitalTwinLogger::Instance()->ReceiveMotionInitialPosition(tubeAngleSensorValue, tubeHeightSensorValue);
	DigitalTwinLogger::Instance()->ReceiveDetectorHeight(detectorHeight);

	AlignTubeHeightToDetector();
	RotateTubeToExposure();
}

void RADMotionModel::AlignTubeHeightToDetector()
{
	float despos = 1.0;
	float tubeCurPos = 1.0;

	if (m_CurWS == WS_WALL)
	{
		despos = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
		tubeCurPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
	}
	else if (m_CurWS == WS_TABLE)
	{
		despos = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HORIZONTAL);
		tubeCurPos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
	}

	if (m_technicalParams.GetFirstOf("IsTubeSameHeightWithDetector") >= 0)
	{
		if (!atoi(m_technicalParams["IsTubeSameHeightWithDetector"]))
		{
			if (m_technicalParams.GetFirstOf("TubeDesHeight") >= 0)
			{
				auto desHeigt = strtof((const char*)m_technicalParams["TubeDesHeight"], nullptr);
				if (desHeigt >= 1.0f)
				{
					tubeCurPos = desHeigt;
				}
			}
		}
	}

	if (gmotionLog) gmotionLog->Info("[RADMotionModel][AlignTubeToDetector]->[{$}][Tube cur pos:{$:f3}, des pos:{$:f3}]", m_CurWS.c_str(), tubeCurPos, despos);

	if (m_CurWS == WS_WALL)
	{
		int direction = JudgeDirectionInTubeHeightAxis(tubeCurPos, despos);
		MoveTubeHeight(direction, abs(despos - tubeCurPos));
	}
	else if (m_CurWS == WS_TABLE)
	{
		int direction = JudgeDirectionInTubeHorizontalAxis(tubeCurPos, despos);
		MoveTubeHorizontal(direction, abs(despos - tubeCurPos));
	}
}

void RADMotionModel::RotateTubeToExposure()
{
	float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);
	float exposureAngle = GetExposureAngle();

	//if (m_technicalParams.GetFirstOf("TubeDesAngle") >= 0)
	//{
	//	exposureAngle = strtof((const char*)m_technicalParams["TubeDesAngle"], nullptr);
	//}

	if (gmotionLog) gmotionLog->Info("[RADMotionModel][RotateTubeToExposure]->[{$}][Tube des angle:{$:f3}]", m_CurWS.c_str(), exposureAngle);

	int direction = JudgeDirectionInTubeAngleAxis(tubeAngle, exposureAngle);

	RotateTubeAngle(direction, abs(tubeAngle - exposureAngle));
}


void RADMotionModel::MoveTubeHeight(int direction, float offset)
{
	auto tolerance = ConfigurerMotion::GetMotionToleranceLine();
	if (offset < tolerance)
	{
		if (gmotionLog) gmotionLog->Info("[RADMotionModel][MoveTubeHeight]->[ Offset is {$:f6}, less than {$:f6}m, no motion]", offset, tolerance);

		return;
	}

	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(false);
	}

	//float speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
	//auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HEIGHT, speed);
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, offset);
	auto period = ConfigurerMotion::GetTubeHeightNormalPeriod();
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}
	m_motorTubeHeight->Move(direction, step, period);
}

void RADMotionModel::RotateTubeAngle(int direction, float offset)
{
	//auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]);
	auto period = (DWORD)ConfigurerMotion::GetTubeRotateResetPeriod();
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, offset);
	m_motorTubeAngle->Rotate(direction, step, period);
}

float RADMotionModel::GetExposureAngle()
{
	float exposureangle = (float)atof((const char*)m_MachineryParams["TubeAngleOfRAD"]);
	if (m_CurWS == WS_WALL)
	{
		exposureangle = ConfigurerWS::GetDefaultAngleWall();
	}
	if (m_CurWS == WS_TABLE)
	{
		exposureangle = ConfigurerWS::GetDefaultAngleTable();
	}

	if (gmotionLog) gmotionLog->Info("[RADMotionModel][GetExposureAngle]->[{$}][Tube des angle:{$:f3}]", m_CurWS.c_str(), exposureangle);

	return exposureangle;
}

int RADMotionModel::JudgeDirectionInTubeHeightAxis(float current, float target)
{
	//auto positive = (int)atoi((const char *)m_MachineryParams["TubeHeightAxisPositiveDirection"]);
	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 RADMotionModel::JudgeDirectionInTubeAngleAxis(float current, float target)
{
	//auto positive = (int)atoi((const char *)m_MachineryParams["TubeRotateAxisPositiveDirection"]);
	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 RADMotionModel::SwitchScanningComponents(int nSwitch)
{

}

void RADMotionModel::OnMotionStageMoveTubeHeight()
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(false);
	}

	if (m_motorTubeHeight)
	{
		//auto period = (int)atoi((const char*)m_MachineryParams["ResetHeightPeriod"]);
		auto period = ConfigurerMotion::GetTubeHeightResetPeriod();
		if (gmotionLog) gmotionLog->Info("[RADMotionModel][OnMotionStageMoveTubeHeight]->[ Org:{$:d}, Period:{$:d}]", m_stageArgs->TubeHeightDirection, period);

		m_motorTubeHeight->Move(m_stageArgs->TubeHeightDirection, m_stageArgs->TubeHeightStep, period);
	}
}

void RADMotionModel::OnMotionStageMoveTubeHorizontal()
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(true);
	}

	if (m_motorTubeHorizontal)
	{
		//auto period = (int)atoi((const char*)m_MachineryParams["ResetHorizontalPeriod"]);
		auto period = ConfigurerMotion::GetTubeHorizontalResetPeriod();

		if (gmotionLog) gmotionLog->Info("[RADMotionModel][OnMotionStageMoveTubeHorizontal]->[ Org:{$:d}, Period:{$:d}]", m_stageArgs->TubeHorizontalMoveDirection, period);

		m_motorTubeHorizontal->Move(m_stageArgs->TubeHorizontalMoveDirection, m_stageArgs->TubeHorizontalMoveStep, period);
	}
}

void RADMotionModel::OnMotionStageMoveTubeRotation()
{
	if (m_motorTubeAngle)
	{
		//auto period = (int)atoi((const char*)m_MachineryParams["ResetRotatePeriod"]);
		auto period = ConfigurerMotion::GetTubeRotateResetPeriod();
		m_motorTubeAngle->Rotate(m_stageArgs->TubeAngleDirection, m_stageArgs->TubeAngleStep, period);
	}
}

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

int RADMotionModel::JudgeDirectionInTubeHorizontalAxis(float current, float target)
{
	//auto positive = (int)atoi((const char*)m_MachineryParams["TubeHorizontalAxisPositiveDirection"]);
	auto positive = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection();
	auto direction = positive > 0 ? 1 : -1;

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

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

	return 0;
}

void RADMotionModel::MoveTubeHorizontal(int direction, float offset)
{
	auto tolerance = ConfigurerMotion::GetMotionToleranceLine();
	if (offset < tolerance)
	{
		if (gmotionLog) gmotionLog->Info("[RADMotionModel][MoveTubeHorizontal]->[ Offset is {$:f6}, less than {$:f6}m, no motion]", offset, tolerance);

		return;
	}

	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(true);
	}

	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HORIZONTAL, offset);
	
	auto period = ConfigurerMotion::GetTubeHorizontalNormalPeriod();
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}
	m_motorTubeHorizontal->Move(direction, step, period);
}

void RADMotionModel::OnMotionStageMove2SID(string ws)
{
	if (gmotionLog) gmotionLog->Info("[RADMotionModel][OnMotionStageMove2SID]->[{$}]", ws.c_str());

	if (ws == WS_WALL)
	{
		//float desPos = atof((const char*)m_MachineryParams["SID_Wall"]);
		float desPos = ConfigurerWS::GetDefaultSIDWall();
		float tubeHorizontalPos = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HORIZONTAL);
		int direction = JudgeDirectionInTubeHorizontalAxis(tubeHorizontalPos, desPos);

		MoveTubeHorizontal(direction, abs(tubeHorizontalPos - desPos));
	}
	else if (ws == WS_TABLE)
	{
		//float desPos = atof((const char*)m_MachineryParams["SID_Table"]);
		float desPos = ConfigurerWS::GetDefaultSIDTable();
		float tubeHeightPos = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);
		int direction = JudgeDirectionInTubeHeightAxis(tubeHeightPos, desPos);

		MoveTubeHeight(direction, abs(tubeHeightPos - desPos));
	}
}