#include "stdafx.h"
#include "LocProcess.h"
#include "VisionRecipe.h"
#include "BlSideData.h"
#include "FileRecipe.h"

CLocProcess::CLocProcess(){
	/* code */
	for (int i = 0; i < 2; i++) {
		m_dot[i] = NULL;
	}

	m_topMark.x = 0;
	m_topMark.y = 0;
	m_botMark.x = 0;
	m_botMark.y = 0;
	m_botPoint.x = 0;
    m_botPoint.y = 0;
	m_topPoint.x = 0;
    m_topPoint.y = 0;
}

CLocProcess::CLocProcess(DimensionDir eDir) {
	/* code */
	m_eDir = eDir;
	for (int i = 0; i < 2; i++) {
		CNccDotProcess *dot = new CNccDotProcess(m_eDir, i);
		m_dot[i] = dot;
	}

	m_topMark.x = 0;
	m_topMark.y = 0;
	m_botMark.x = 0;
	m_botMark.y = 0;
	m_botPoint.x = 0;
	m_botPoint.y = 0;
	m_topPoint.x = 0;
	m_topPoint.y = 0;
}

CLocProcess::~CLocProcess(){
	/* code */
	Release();
}

void CLocProcess::Release(void) {
	/* code */
	for (int i = 0; i < 2; i++) {
		CNccDotProcess *dot = m_dot[i];
		if (NULL == dot) continue;
		delete dot;
		dot = NULL;
		m_dot[i] = NULL;
	}
}

void CLocProcess::Init(void) {
	/* code */
	for (int i = 0; i < 2; i++) {
		CNccDotProcess *dot = new CNccDotProcess(m_eDir, i);
		m_dot[i] = dot;
	}

	m_topMark.x = 0;
	m_topMark.y = 0;
	m_botMark.x = 0;
	m_botMark.y = 0;
}

CNccDotProcess* CLocProcess::getDot(int index) {
	/* code */
	if (index < 0 || index > 1) return NULL;

	CNccDotProcess *dot = m_dot[index];
	return dot;
}

void CLocProcess::setPose(Point2I pose, int nType) {
	/* code */
	CBlSideData *pSideData = CVisionRecipe::getInstance()->getSideData(m_eDir);
	if (nullptr == pSideData) return;

	double dMmvspX = pSideData->m_dPixelSizeX;   //X·½ÏòµÄÏñËØ´óС
	double dMmvspY = pSideData->m_dPixelSizeY;   //Y·½ÏòµÄÏñËØ´óС

	Point2I point;
	point.x = (int)(pose.x * dMmvspX);
    point.y = (int)(pose.y * dMmvspY);
	if (0 == nType) {
		m_topMark = point;
        pSideData->m_mTopBzMark = point;
	}
	else if (1 == nType){
		m_botMark = point;
		pSideData->m_mBotBzMark = point;
	}
	else if (2 == nType) {
		m_topPoint = point;
		pSideData->m_mTopBzPoint = point;
	}
	else if (3 == nType) {
        m_botPoint = point;
        pSideData->m_mBotBzPoint = point;
	}
}

Point2I CLocProcess::getPose(int nType) {
	/* code */
	if (0 == nType) {
		return m_topMark;
	}
	else if (1 == nType) {
		return m_botMark;
	}
	else if (2 == nType) {
        return m_topPoint;
	}
	else if (3 == nType) {
        return m_botPoint;
	}

	return m_topMark;
}

int CLocProcess::execute(DimensionDir eDir) {
	/* code */
	CBlSideData *pSideData = CVisionRecipe::getInstance()->getSideData(m_eDir);
	if (NULL == pSideData) return 0;

	pSideData->m_bTopPoint_Find = FALSE;
	pSideData->m_bBotPoint_Find = FALSE;
//	Point2I offset, pose;
	Point2I offset;
	for (int i = 0; i < 2; i++) {
		CNccDotProcess *dot = m_dot[i];
		if (NULL == dot) continue;

		SetPos(dot);
//		pose = dot->getPose();
//		offset = getOffset(pose);
		offset.x = 0;
        offset.y = 0;

		dot->execute(eDir, offset);

		SetResult(dot);
	}
	return 0;
}

void CLocProcess::SetPos(CNccDotProcess* dot) {
	/* code */
	CBlSideData *pSideData = CVisionRecipe::getInstance()->getSideData(m_eDir);
	if (NULL == pSideData) return;
    if (NULL == dot) return;

	Point2I pose = GetPos(dot);
	if (0 == pose.x && 0 == pose.y) return;

	double dx = pose.x - m_topMark.x;
	double dy = pose.y - m_topMark.y;
	double dist0 = sqrt(dx * dx + dy * dy);
	dx = pose.x - m_botMark.x;
    dy = pose.y - m_botMark.y;
    double dist1 = sqrt(dx * dx + dy * dy);

	double dMmvspX = pSideData->m_dPixelSizeX;   //X·½ÏòµÄÏñËØ´óС
	double dMmvspY = pSideData->m_dPixelSizeY;   //Y·½ÏòµÄÏñËØ´óС

	if (dist0 < dist1) {
		if (!pSideData->m_bTopMark_Find)  return;

		Point2I topMark = pSideData->m_mTopMark;

		Point2D offset;
		offset.x = dMmvspX * topMark.x - m_topMark.x;
		offset.y = dMmvspY * topMark.y - m_topMark.y;

		Point2I locPose;
		locPose.x = (int)((pose.x + offset.x) / dMmvspX);
        locPose.y = (int)((pose.y + offset.y) / dMmvspY);
		dot->setPose(locPose);
	}
	else {
		if (!pSideData->m_bBotMark_Find) return;
		Point2I botMark = pSideData->m_mBotMark;

        Point2D offset;
		offset.x = dMmvspX * botMark.x - m_botMark.x;
        offset.y = dMmvspY * botMark.y - m_botMark.y;

		Point2I locPose;
		locPose.x = (int)((pose.x + offset.x) / dMmvspX);
		locPose.y = (int)((pose.y + offset.y) / dMmvspY);
		dot->setPose(locPose);
	}
}

Point2I CLocProcess::GetPos(CNccDotProcess* dot) {
	/* code */
	Point2I pose;
	pose.x = 0;
    pose.y = 0;
	if (0 == dot->getID()) {
		pose = m_topPoint;
	}
	else {
		pose = m_botPoint;
	}

	return pose;
}

void CLocProcess::SetResult(CNccDotProcess *dot) {
	/* code */
	if (NULL == m_dot) return;

	CBlSideData *pSideData = CVisionRecipe::getInstance()->getSideData(m_eDir);
	if (NULL == pSideData) return;

	int id = dot->getID();
	if (0 == id) {
		if (fabs(dot->m_ptResult.x) < 0.01 && fabs(dot->m_ptResult.y) < 0.01) return;

		pSideData->m_bTopPoint_Find = TRUE;
		pSideData->m_mTopPoint.x = (int)(dot->m_ptResult.x);
		pSideData->m_mTopPoint.y = (int)(dot->m_ptResult.y);
	}
	else if (1 == id) {
		if (fabs(dot->m_ptResult.x) < 0.01 && fabs(dot->m_ptResult.y) < 0.01) return;

		pSideData->m_bBotPoint_Find = TRUE;
		pSideData->m_mBotPoint.x = (int)(dot->m_ptResult.x);
		pSideData->m_mBotPoint.y = (int)(dot->m_ptResult.y);
	}
}

Point2I CLocProcess::getOffset(Point2I pose) {
	/* code */
	Point2I result;
	result.x = 0;
	result.y = 0;
	CBlSideData *pSideData = CVisionRecipe::getInstance()->getSideData(m_eDir);
	if (NULL == pSideData) return result;
	if (0 == pose.x && 0 == pose.y) return result;

	double dx = pose.x - m_topMark.x;
	double dy = pose.y - m_topMark.y;
	double dist0 = sqrt(dx * dx + dy * dy);
	dx = pose.x - m_botMark.x;
	dy = pose.y - m_botMark.y;
	double dist1 = sqrt(dx * dx + dy * dy);

	if (dist0 < dist1) {
		if (pSideData->m_bTopMark_Find) {
			result.x = pSideData->m_mTopMark.x - m_topMark.x;
			result.y = pSideData->m_mTopMark.y - m_topMark.y;
		}
		else if (pSideData->m_bBotMark_Find) {
			result.x = pSideData->m_mBotMark.x - m_botMark.x;
			result.y = pSideData->m_mBotMark.y - m_botMark.y;
		}
	}
	else {
		if (pSideData->m_bBotMark_Find) {
			result.x = pSideData->m_mBotMark.x - m_botMark.x;
			result.y = pSideData->m_mBotMark.y - m_botMark.y;
		}
		else if(pSideData->m_bTopMark_Find) {
			result.x = pSideData->m_mTopMark.x - m_topMark.x;
			result.y = pSideData->m_mTopMark.y - m_topMark.y;
		}
	}

	return result;
}

Json::Value CLocProcess::WriteToJson(std::string &strDir) {
	/* code */
	Json::Value jsValue;
	jsValue["type"] = LOC_VISION_PROCESS;
	jsValue["side"] = (int)(m_eDir);
	jsValue["TopMark X"] = m_topMark.x;
	jsValue["TopMark Y"] = m_topMark.y;
	jsValue["BotMark X"] = m_botMark.x;
	jsValue["BotMark Y"] = m_botMark.y;
	jsValue["TopPoint X"] = m_topPoint.x;
    jsValue["TopPoint Y"] = m_topPoint.y;
    jsValue["BotPoint X"] = m_botPoint.x;
	jsValue["BotPoint Y"] = m_botPoint.y;


	for (int i = 0; i < 2; i++) {
		std::string name = ClsVision::FormatString("Dot%d", i);
		jsValue[name.c_str()] = m_dot[i]->WriteToJson(strDir);
	}
	return jsValue;
}

int CLocProcess::DecodeJson(std::string &strDir, Json::Value &jsValue) {
	/* code */
	int num = (int)jsValue.size();
	if (num < 1) return -1;

	CBlSideData *pSideData = CVisionRecipe::getInstance()->getSideData(m_eDir);
	if (nullptr == pSideData) return -1;

	//1. Side
	std::string strName = "side";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_eDir = (DimensionDir)(jsValue[strName.c_str()].asInt());
	}

	//2. TopMark X,//3. TopMark Y
	strName = "TopMark X";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_topMark.x = jsValue[strName.c_str()].asInt();
		pSideData->m_mTopBzMark.x = m_topMark.x;
	}
	strName = "TopMark Y";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_topMark.y = jsValue[strName.c_str()].asInt();
		pSideData->m_mTopBzMark.y = m_topMark.y;
	}
	strName = "BotMark X";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_botMark.x = jsValue[strName.c_str()].asInt();
		pSideData->m_mBotBzMark.x = m_botMark.x;
	}
	strName = "BotMark Y";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_botMark.y = jsValue[strName.c_str()].asInt();
		pSideData->m_mBotBzMark.y = m_botMark.y;
	}

	strName = "TopPoint X";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_topPoint.x = jsValue[strName.c_str()].asInt();
		pSideData->m_mTopBzPoint.x = m_topPoint.x;
	}
	strName = "TopPoint Y";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_topPoint.y = jsValue[strName.c_str()].asInt();
        pSideData->m_mTopBzPoint.y = m_topPoint.y;
	}
	strName = "BotPoint X";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_botPoint.x = jsValue[strName.c_str()].asInt();
        pSideData->m_mBotBzPoint.x = m_botPoint.x;
	}
	strName = "BotPoint Y";
	if (jsValue.isMember(strName.c_str()) && jsValue[strName.c_str()].isInt()) {
		m_botPoint.y = jsValue[strName.c_str()].asInt();
        pSideData->m_mBotBzPoint.y = m_botPoint.y;
	}

	for (int i = 0; i < 2; i++) {
		std::string name = ClsVision::FormatString("Dot%d", i);
		if (jsValue.isMember(name.c_str()) && jsValue[name.c_str()].isObject()) {
			Json::Value jsData = jsValue[name.c_str()];
			m_dot[i]->DecodeJson(strDir, jsData);
		}
	}

	return 0;
}