#include "stdafx.h"
#include "CalibrationModel.h"
#include "MotionStages.h"
#include "CalibrationStageArgs.h"
#include "IMachineryManager.h"
#include "ITubeAngleController.h"
#include "ITubeHeightController.h"
#include "ISensorADController.h"
#include "ISensorEncoderController.h"
#include "IPositionManager.h"
#include "ConfigurerMotion.h"
#include "TubeLineMotionSwitchController.h"

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

CalibrationModel::CalibrationModel()
:m_StageName(""),
m_stageArgs(new CalibrationStageArgs()),
m_coordinates(nullptr),
m_tubeAngle(nullptr),
m_tubeHeight(nullptr),
m_tubeHeightAD(nullptr),
m_tubeAngleAD(nullptr),
m_detectorAD(nullptr),
m_tubeHeightEncoder(nullptr),
m_tubeAngleEncoder(nullptr),
m_detectorHeightEncoder(nullptr),
m_motorTubeHorizontal(nullptr),
m_encoderTubeHorizontal(nullptr),
m_tubeLineMotionSwitch(nullptr),
m_tubeHeightMotionPeriod(150),
m_tubeAngleMotionPeriod(150),
m_tubeRotateLandmarkDirection(1),
m_tubeHeightLandmarkDirection(1),
m_tubeHeightAxisPositiveDirection(1),
m_tubeRotateAxisPositiveDirection(1),
m_autoCalADPhysicalEachStep(0.0f)
{
}


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

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

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

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

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

	m_tubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
	m_tubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));
	m_detectorAD = (ISensorADController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_AD));
	m_tubeHeightAD = (ISensorADController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_AD));
	m_tubeAngleAD = (ISensorADController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_AD));
	m_tubeHeightEncoder = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT_ENCODER));
	m_tubeAngleEncoder = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE_ENCODER));
	m_detectorHeightEncoder = (ISensorEncoderController *)(machineryManager->Resove(CONTROLLER_DETECTOR_HEIGHT_ENCODER));

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

	m_detectorHorizontalAD = (ISensorADController*)(machineryManager->Resove(CONTROLLER_DETECTOR_HORIZONTAL_AD));
	m_detectorHorizontalEncoder = (ISensorEncoderController*)(machineryManager->Resove(CONTROLLER_DETECTOR_HORIZONTAL_ENCODER));

	assert(m_tubeAngle);
	assert(m_tubeHeight);
	assert(m_detectorAD);
	assert(m_tubeHeightAD);
	assert(m_tubeAngleAD);
	assert(m_tubeHeightEncoder);
	assert(m_tubeAngleEncoder);
	assert(m_detectorHeightEncoder);
}

void CalibrationModel::LoadMachineryParams(ResDataObject &params)
{
	m_tubeAngleMotionPeriod = (DWORD)ConfigurerMotion::GetTubeRotateResetPeriod();
	m_tubeRotateLandmarkDirection = ConfigurerMotion::GetTubeRotateLandmarkDirection();
	m_tubeRotateAxisPositiveDirection = ConfigurerMotion::GetTubeRotateAxisPositiveDirection() > 0 ? 1 : -1;

	m_tubeHeightMotionPeriod = (DWORD)ConfigurerMotion::GetTubeHeightResetPeriod();
	m_tubeHeightLandmarkDirection = ConfigurerMotion::GetTubeHeightLandmarkDirection();
	m_tubeHeightAxisPositiveDirection = ConfigurerMotion::GetTubeHeightAxisPositiveDirection() > 0 ? 1 : -1;

	m_tubeHorizontalMotionPeriod = (DWORD)ConfigurerMotion::GetTubeHorizontalResetPeriod();
	m_tubeHorizontalLandmarkDirection = ConfigurerMotion::GetTubeHorizontalLandmarkDirection();
	m_tubeHorizontalAxisPositiveDirection = ConfigurerMotion::GetTubeHorizontalAxisPositiveDirection() > 0 ? 1 : -1;
}

void CalibrationModel::LoadModelParams(ResDataObject &params)
{

}

void CalibrationModel::SetTechnicalParams(ResDataObject &params)
{

}

void CalibrationModel::OnFeedbackMotionParams(ResDataObject &params)
{

}

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

void CalibrationModel::OnMotionStage(const std::string &name)
{
	if (CALIBRATION_MOVE_TUBE_HEIGHT == name)
	{
		OnStageMoveTubeHeight();
	}
	else if (name == CALIBRATION_ROTATE_TUBE_ANGLE)
	{
		OnStageRotateTubeAngle();
	}
	else if (name == CALIBRATION_GET_TUBE_ANGLE_AD)
	{
		OnStageGetTubeAngleAD();
	}
	else if (name == CALIBRATION_GET_TUBE_HEIGHT_AD)
	{
		OnStageGetTubeHeightAD();
	}
	else if (name == CALIBRATION_GET_DETECTOR_HEIGHT_AD)
	{
		OnStageGetDetectorHeightAD();
	}
	else if (name == CALIBRATION_GET_TUBE_ANGLE_ENCODER)
	{
		OnStageGetTubeAngleEncoder();
	}
	else if (name == CALIBRATION_GET_TUBE_HEIGHT_ENCODER)
	{
		OnStageGetTubeHeightEncoder();
	}
	else if (name == CALIBRATION_GET_DETECTOR_HEIGHT_ENCODER)
	{
		OnStageGetDetectorHeightEncoder();
	}
	else if (name == CALIBRATION_ACTIVE_TUBE_ANGLE_ZAXIS_CLEAR)
	{
		OnStageActiveTubeAngleZAxisClear();
	}
	else if (name == CALIBRATION_CANCEL_TUBE_ANGLE_ZAXIS_CLEAR)
	{
		OnStageCancelTubeAngleZAxisClear();
	}
	else if (name == CALIBRATION_ACTIVE_TUBE_HEIGHT_ZAXIS_CLEAR)
	{
		OnStageActiveTubeHeightZAxisClear();
	}
	else if (name == CALIBRATION_CANCEL_TUBE_HEIGHT_ZAXIS_CLEAR)
	{
		OnStageCancelTubeHeightZAxisClear();
	}
	else if (name == CALIBRATION_ROTATE_TUBE_ANGLE_TO_ZAXIS)
	{
		OnStageRotateTubeAngleToZAxis();
	}
	else if (name == CALIBRATION_MVOE_TUBE_HEIGHT_TO_ZAXIS)
	{
		OnStageMoveTubeHeightToZAxis();
	}
	else if (name == CALIBRATION_STOP_TUBE_ANGLE_AT_ZAXIS)
	{
		OnStageStopTubeAngleAtZxis();
	}
	else if (name == CALIBRATION_STOP_TUBE_HEIGHT_AT_ZAXIS)
	{
		OnStageStopTubeHeightAtZAxis();
	}
	else if (name == CALIBRATION_MOVE_TUBE_HEIGHT_AS_PHYSICAL)
	{
		OnStageMoveTubeHeightAsPhysical();
	}
	else if (name == CALIBRATION_ROTATE_TUBE_ANGLE_AS_PHYSICAL)
	{
		OnStageRotateTubeAngleAsPhysical();
	}
	else if (name == CALIBRATION_SWITCH_SVO_STATUS)
	{
		OnStageSwitchSvoStatus();
	}
	else if (name == CALIBRATION_AUTO_CALIBRATION_AD_START)
	{
		OnStageAutoCalibrationADStart();
	}
	else if (name == CALIBRATION_AUTO_CALIBRATION_AD_NEXT)
	{
		OnStageAutoCalibrationADNext();
	}
	else if (name == CALIBRATION_AUTO_CALIBRATION_AD_END)
	{
		OnStageAutoCalibrationADEnd();
	}
	else if (name == CALIBRATION_MOVE_TUBE_HORIZONTAL)
	{
		OnStageMoveTubeHorizontal();
	}
	else if (name == CALIBRATION_GET_TUBE_HORIZONTAL_AD)
	{
		OnStageGetTubeHorizontalAD();
	}
	else if (name == CALIBRATION_GET_TUBE_HORIZONTAL_ENCODER)
	{
		OnStageGetTubeHorizontalEncoder();
	}
	else if (name == CALIBRATION_GET_DETECTOR_HORIZONTAL_AD)
	{
		OnStageGetDetectorHorizontalAD();
	}
}

void CalibrationModel::OnStageMoveTubeHeight()
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(false);
	}

	m_tubeHeight->Move(
		m_stageArgs->TubeHeightMoveDirection,
		m_stageArgs->TubeHeightMoveStep,
		m_tubeHeightMotionPeriod
		);
}

void CalibrationModel::OnStageRotateTubeAngle()
{
	m_tubeAngle->Rotate(
		m_stageArgs->TubeAngleRotateDirection, 
		m_stageArgs->TubeAngleRotateStep,
		m_tubeAngleMotionPeriod);
}

void CalibrationModel::OnStageMoveTubeHeightAsPhysical()
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(false);
	}

	auto step = m_coordinates->ConvertMotorStepValue(m_tubeHeight->Name(), m_stageArgs->TubeHeightMovePhysical);
	m_tubeHeight->Move(
		m_stageArgs->TubeHeightMoveDirection,
		step,
		m_tubeHeightMotionPeriod
		);
}

void CalibrationModel::OnStageRotateTubeAngleAsPhysical()
{
	auto step = m_coordinates->ConvertMotorStepValue(m_tubeAngle->Name(), m_stageArgs->TubeAngleRotatePhysical);
	m_tubeAngle->Rotate(
		m_stageArgs->TubeAngleRotateDirection,
		step,
		m_tubeAngleMotionPeriod);
}

void CalibrationModel::OnStageGetTubeAngleAD()
{
	auto ad = m_tubeAngleAD->GetCurrentADValue();
	m_stageArgs->TubeAngleAD = ad;
}

void CalibrationModel::OnStageGetTubeHeightAD()
{
	auto ad = m_tubeHeightAD->GetCurrentADValue();
	m_stageArgs->TubeHeightAD = ad;
}

void CalibrationModel::OnStageGetDetectorHeightAD()
{
	auto ad = m_detectorAD->GetCurrentADValue();
	m_stageArgs->DetectorHeightAD = ad;
}

void CalibrationModel::OnStageGetTubeAngleEncoder()
{
	auto encoder = m_tubeAngleEncoder->GetCurrentEncoderValue();
	m_stageArgs->TubeAngleEncoder = encoder;
}

void CalibrationModel::OnStageGetTubeHeightEncoder()
{
	auto encoder = m_tubeHeightEncoder->GetCurrentEncoderValue();
	m_stageArgs->TubeHeightEncoder= encoder;
}

void CalibrationModel::OnStageGetDetectorHeightEncoder()
{
	auto encoder = m_detectorHeightEncoder->GetCurrentEncoderValue();
	m_stageArgs->DetectorHeightEncoder = encoder;
}

void CalibrationModel::OnStageActiveTubeAngleZAxisClear()
{
	m_tubeAngleEncoder->BindZAxis();
	m_tubeAngleEncoder->ActiveAutoNotifyWhenZClear(true);
}

void CalibrationModel::OnStageCancelTubeAngleZAxisClear()
{
	m_tubeAngleEncoder->UnBindZAxis();
	m_tubeAngleEncoder->ActiveAutoNotifyWhenZClear(false);
}

void CalibrationModel::OnStageActiveTubeHeightZAxisClear()
{
	m_tubeHeightEncoder->BindZAxis();
	m_tubeHeightEncoder->ActiveAutoNotifyWhenZClear(true);
}

void CalibrationModel::OnStageCancelTubeHeightZAxisClear()
{
	m_tubeHeightEncoder->UnBindZAxis();
	m_tubeHeightEncoder->ActiveAutoNotifyWhenZClear(false);
}

void CalibrationModel::OnStageRotateTubeAngleToZAxis()
{
	m_tubeAngleEncoder->BindZAxis();
	m_tubeAngleEncoder->ActiveAutoNotifyWhenZClear(true);
	int direction = m_tubeRotateAxisPositiveDirection > 0 ? 1 : -1;

	direction = m_tubeRotateLandmarkDirection > 0 ? direction : (-1 * direction);
	direction = direction * -1;

	m_tubeAngle->Rotate(direction, -1, m_tubeAngleMotionPeriod);
}

void CalibrationModel::OnStageMoveTubeHeightToZAxis()
{
	m_tubeHeightEncoder->BindZAxis();
	m_tubeHeightEncoder->ActiveAutoNotifyWhenZClear(true);

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

	//direction = m_tubeHeightLandmarkDirection > 0 ? direction : (-1 * direction);
	direction = m_tubeHeightLandmarkDirection > 0 ? (-1 * direction) : direction;
	direction = direction * -1;

	m_tubeHeight->Move(direction, -1, m_tubeHeightMotionPeriod);
}

void CalibrationModel::OnStageStopTubeAngleAtZxis()
{
	m_tubeAngle->StopRotation();
}

void CalibrationModel::OnStageStopTubeHeightAtZAxis()
{
	m_tubeHeight->StopMove();
}

void CalibrationModel::OnStageSwitchSvoStatus()
{
	int targetStatus = m_stageArgs->SvoStatus;
	if (m_stageArgs->AutoCalADDof == 0)
	{
		m_tubeAngle->SetServoStatus(targetStatus);
	}
	else if (m_stageArgs->AutoCalADDof == 1)
	{
		m_tubeHeight->SetServoStatus(targetStatus);
	}
}

void CalibrationModel::OnStageAutoCalibrationADStart()
{
	m_stageArgs->AutoCalADPhysicals.clear();
	m_stageArgs->AutoCalADADs.clear();

	auto hlim = m_stageArgs->AutoCalADHighLimit;
	auto llim = m_stageArgs->AutoCalADLowLimit;
	m_autoCalADPhysicalEachStep = (hlim - llim) / (m_stageArgs->AutoCalADTotalStep);

	//Move to start pos(high limit)
	float current = m_stageArgs->AutoCalADCurrentPhysical;
	float target = m_stageArgs->AutoCalADHighLimit;

	MoveMech(m_stageArgs->AutoCalADDof,current, target);
}

void CalibrationModel::OnStageAutoCalibrationADNext()
{
	m_stageArgs->AutoCalADPhysicals.push_back(m_stageArgs->AutoCalADHighLimit - (m_autoCalADPhysicalEachStep * m_stageArgs->AutoCalADCurrentStep));
	m_stageArgs->AutoCalADADs.push_back(GetAD(m_stageArgs->AutoCalADDof) * 1.0f);

	//Move to next pos
	float current = m_stageArgs->AutoCalADHighLimit - (m_autoCalADPhysicalEachStep * m_stageArgs->AutoCalADCurrentStep);
	float target = current - m_autoCalADPhysicalEachStep;

	MoveMech(m_stageArgs->AutoCalADDof,current, target);
}

void CalibrationModel::OnStageAutoCalibrationADEnd()
{

}

int  CalibrationModel::GetAD(int dof)
{
	if (dof == 0)
	{
		return m_tubeAngleAD->GetCurrentADValue();
	}
	else if (dof == 1)
	{
		return m_tubeHeightAD->GetCurrentADValue();
	}

	return 0;
}

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

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

void CalibrationModel::MoveTubeHeight(float current, float target)
{
	float offset = abs(current - target);

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

int CalibrationModel::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 CalibrationModel::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 CalibrationModel::SwitchScanningComponents(int nSwitch)
{

}

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


void CalibrationModel::OnStageMoveTubeHorizontal()
{
	if (m_tubeLineMotionSwitch)
	{
		m_tubeLineMotionSwitch->OutputSignal(true);
	}

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

void CalibrationModel::OnStageGetTubeHorizontalAD()
{
	auto ad = m_adTubeHorizontal->GetCurrentADValue();
	m_stageArgs->TubeHorizontalAD = ad;
}

void CalibrationModel::OnStageGetTubeHorizontalEncoder()
{
	auto encoder = m_encoderTubeHorizontal->GetCurrentEncoderValue();
	m_stageArgs->TubeHorizontalEncoder = encoder;
}

void CalibrationModel::OnStageGetDetectorHorizontalAD()
{
	auto ad = m_detectorHorizontalAD->GetCurrentADValue();
	m_stageArgs->DetectorHorizontalAD = ad;
}