#include "StdAfx.h"
#include "MotorCalculator.h"
#include <math.h>

CMotorCalculator::CMotorCalculator(int nIndex)
{
	m_nIndex = nIndex;
	Reset();
}

CMotorCalculator::~CMotorCalculator(void)
{
	Reset();
}

void CMotorCalculator::Reset()
{
	m_vectorDesignPos.clear();
	m_vectorMotorPos.clear();
	m_sCalculateResult.Reset();
}

void CMotorCalculator::SetDesignPosition(const VectorDouble& vectorPosX, const VectorDouble& vectorPosY)
{
	if (vectorPosX.size()!=3) return;

	if (vectorPosX.size()!=vectorPosY.size()) return;

	m_vectorDesignPos.clear();

	for (int i=0; i<3; i++)
	{
		m_vectorDesignPos.push_back(SPosition2D(vectorPosX[i], vectorPosY[i]));
	}
}

void CMotorCalculator::SetMotorPosition(const VectorDouble& vectorPosX, const VectorDouble& vectorPosY)
{
	if (vectorPosX.size()!=3) return;

	if (vectorPosX.size()!=vectorPosY.size()) return;

	m_vectorMotorPos.clear();

	for (int i=0; i<3; i++)
	{
		m_vectorMotorPos.push_back(SPosition2D(vectorPosX[i], vectorPosY[i]));
	}
}

int CMotorCalculator::CalculateResult()
{
	if (m_vectorDesignPos.size()!=3) return 0;

	if (m_vectorDesignPos.size()!=m_vectorMotorPos.size()) return 0;

	m_sCalculateResult.dStraghtValue = m_vectorMotorPos[0].dPositionY - m_vectorMotorPos[1].dPositionY;

	m_sCalculateResult.dStraightLineA = (m_vectorMotorPos[1].dPositionY-m_vectorMotorPos[0].dPositionY) / (m_vectorMotorPos[1].dPositionX-m_vectorMotorPos[0].dPositionX);
	m_sCalculateResult.dStraightLineB = ((m_vectorMotorPos[1].dPositionX*m_vectorMotorPos[0].dPositionY)-(m_vectorMotorPos[0].dPositionX*m_vectorMotorPos[1].dPositionY)) / (m_vectorMotorPos[1].dPositionX-m_vectorMotorPos[0].dPositionX);

	m_sCalculateResult.dOrthogonalLineA = (m_vectorMotorPos[2].dPositionY-m_vectorMotorPos[1].dPositionY) / (m_vectorMotorPos[2].dPositionX-m_vectorMotorPos[1].dPositionX);
	m_sCalculateResult.dOrthogonalLineB = ((m_vectorMotorPos[2].dPositionX*m_vectorMotorPos[1].dPositionY)-(m_vectorMotorPos[1].dPositionX*m_vectorMotorPos[2].dPositionY)) / (m_vectorMotorPos[2].dPositionX-m_vectorMotorPos[1].dPositionX);

	// theta
	m_sCalculateResult.dOrthogonalTheta = atan((m_sCalculateResult.dStraightLineA-m_sCalculateResult.dOrthogonalLineA) / (1.0+(m_sCalculateResult.dStraightLineA*m_sCalculateResult.dOrthogonalLineA)));
	


	
	return 1;
}