#include "stepmotos.h"
#include "inout.h"

//------------------------------------------------------------------------


void DefaultMotoCoilPwm(int coil){};
void DefaultMotoCoilCtrl(void){};

#define FULL_STEP_VAL			(1.0f*MOTO_PPC/FULL_STEP_PPC)		// 整步电子齿轮值
#define DEF_MICO_STEP_VAL		(1.0f*MOTO_PPC/MICO_STEP_PPC)		// 默认微步电子齿轮值


//------------------------------------------------------------------------
//------------------------------------------------------------------------

typedef struct
{
	POS_TYPE motoPos;			// 电机位置

	POS_TYPE fullStepAng;		// 整步电子齿轮值
	POS_TYPE micoStepAng;		// 可保存电子齿轮值

	int ccwdir;

	void (*MotoCoilAPwm)(int);
	void (*SetMotoACPOn)(void);
	void (*SetMotoACPOff)(void);

	void (*SetMotoACNOn)(void);
	void (*SetMotoACNOff)(void);

	void (*MotoCoilBPwm)(int);
	void (*SetMotoBCPOn)(void);
	void (*SetMotoBCPOff)(void);

	void (*SetMotoBCNOn)(void);
	void (*SetMotoBCNOff)(void);

}StepMotoCtrl;

StepMotoCtrl g_stepMotoCtrl[STEP_MOTOS_NUM];

//------------------------------------------------------------------------

void InitStepMotos(void)
{
	memset(g_stepMotoCtrl, 0, sizeof(StepMotoCtrl)*STEP_MOTOS_NUM);

	for (int i = 0; i < STEP_MOTOS_NUM; i++)
	{
		g_stepMotoCtrl[i].motoPos = 0;				// 电机位置
		g_stepMotoCtrl[i].ccwdir = CCWDIR_POSI;				// 电机方向

		g_stepMotoCtrl[i].fullStepAng = FULL_STEP_VAL;			// 整步电子齿轮值
		g_stepMotoCtrl[i].micoStepAng = DEF_MICO_STEP_VAL;		// 可保存电子齿轮值

		g_stepMotoCtrl[i].MotoCoilAPwm = DefaultMotoCoilPwm;
		g_stepMotoCtrl[i].SetMotoACPOn = DefaultMotoCoilCtrl;
		g_stepMotoCtrl[i].SetMotoACPOff = DefaultMotoCoilCtrl;
		g_stepMotoCtrl[i].SetMotoACNOn = DefaultMotoCoilCtrl;
		g_stepMotoCtrl[i].SetMotoACNOff = DefaultMotoCoilCtrl;

		g_stepMotoCtrl[i].MotoCoilBPwm= DefaultMotoCoilPwm;
		g_stepMotoCtrl[i].SetMotoBCPOn = DefaultMotoCoilCtrl;
		g_stepMotoCtrl[i].SetMotoBCPOff = DefaultMotoCoilCtrl;
		g_stepMotoCtrl[i].SetMotoBCNOn = DefaultMotoCoilCtrl;
		g_stepMotoCtrl[i].SetMotoBCNOff = DefaultMotoCoilCtrl;
	}
}

//------------------------------------------------------------------------

void SetMotoCtrlFuncs(int motoIdx,
		MotoCoilPwm coilapwm, MotoCoilCtrl acpon, MotoCoilCtrl acpoff, MotoCoilCtrl acnon, MotoCoilCtrl acnoff,
		MotoCoilPwm coilbpwm, MotoCoilCtrl bcpon, MotoCoilCtrl bcpoff, MotoCoilCtrl bcnon, MotoCoilCtrl bcnoff)
{
	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	pCtrl->MotoCoilAPwm = coilapwm;
	pCtrl->SetMotoACPOn = acpon;
	pCtrl->SetMotoACPOff = acpoff;
	pCtrl->SetMotoACNOn = acnon;
	pCtrl->SetMotoACNOff = acnoff;

	pCtrl->MotoCoilBPwm= coilbpwm;
	pCtrl->SetMotoBCPOn = bcpon;
	pCtrl->SetMotoBCPOff = bcpoff;
	pCtrl->SetMotoBCNOn = bcnon;
	pCtrl->SetMotoBCNOff = bcnoff;
}

void SetMotoCCWDir(int motoIdx, int dir)
{
	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	if (dir >= 0)
	{
		pCtrl->ccwdir = CCWDIR_POSI;
	}
	else
	{
		pCtrl->ccwdir = CCWDIR_NEGA;
	}
}

//------------------------------------------------------------------------
// 步进电机控制输出
// theta 电机角度，每圈  200K个位置 （或 50K 个)
// effort 电流

int StepMotoOut(int motoIdx, int theta, int effort)
{
	int angleA, angleB;
	int coilA, coilB;
	int sinA, sinB;
	StepMotoCtrl * pCtrl;

	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return 0;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);


	if (effort < 0)
	{
		effort = -effort;
	}
	if (effort > MAX_OUT_LEV)
	{
		effort = MAX_OUT_LEV;
	}

	theta *= pCtrl->ccwdir;
	angleA = theta % SIN_TAB_SIZE;			// ((int)(theta * PHASE_MUTI)) % SIN_TAB_SIZE;
	angleB = angleA + (SIN_TAB_SIZE/4);		// 相位角相差 90 度

	sinA = GetSinValue(angleA);
	sinB = GetSinValue(angleB);

	coilA = (int)(effort * sinA / SIN_ITEM_MUTI);
	coilB = (int)(effort * sinB / SIN_ITEM_MUTI);

	if (coilA > 0)
	{
		pCtrl->MotoCoilAPwm(coilA);
		pCtrl->SetMotoACNOn();
		pCtrl->SetMotoACPOff();
	}
	else if (coilA < 0)
	{
		pCtrl->MotoCoilAPwm(-coilA);
		pCtrl->SetMotoACPOn();
		pCtrl->SetMotoACNOff();
	}
	else
	{
		pCtrl->MotoCoilAPwm(0);
		pCtrl->SetMotoACNOn();
		pCtrl->SetMotoACPOn();
	}

	if (coilB > 0)
	{
		pCtrl->MotoCoilBPwm(coilB);
		pCtrl->SetMotoBCNOn();
		pCtrl->SetMotoBCPOff();
	}
	else if (coilB < 0)
	{
		pCtrl->MotoCoilBPwm(-coilB);
		pCtrl->SetMotoBCPOn();
		pCtrl->SetMotoBCNOff();
	}
	else
	{
		pCtrl->MotoCoilBPwm(0);
		pCtrl->SetMotoBCNOn();
		pCtrl->SetMotoBCPOn();
	}
#if (0)
	printf("StepMotoOut, idx=%d, theta=%d, effort=%d\r\n", motoIdx, theta, effort);
	printf("angleA=%d, sinA=%d, coilA=%d\r\n", angleA, sinA, coilA);
	printf("angleB=%d, sinB=%d, coilB=%d\r\n", angleB, sinB, coilB);
#endif

	return 0;
}

//--------------------------------------------------------------

void ResetMotoPos(int motoIdx, int eff)
{
	int pos;

	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	pCtrl->motoPos = 0;
	pos = (int)pCtrl->motoPos;
	StepMotoOut(motoIdx, pos, eff);
}

void SetMotoEnable(int motoIdx, int eff)
{
	int pos;
	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	pos = (int)pCtrl->motoPos;
	StepMotoOut(motoIdx, pos, eff);
}

void SetMotoPosTo(int motoIdx, POS_TYPE pos, int eff)
{
	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	pCtrl->motoPos = pos;
	StepMotoOut(motoIdx, pos, eff);
}

POS_TYPE GetMotoPos(int motoIdx)
{
	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return 0;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	return pCtrl->motoPos;
}

//--------------------------------------------------------------
// 电机走一整步1.8度函数
// dir 方向。 1,  正向; -1, 反向; 0 不走
// eff 电流值
int MotoOneFullStep(int motoIdx, int dir, int eff)
{
	int pos;

	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return 0;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	if (dir > 0)
	{
		pCtrl->motoPos += pCtrl->fullStepAng;
	}
	else if (dir < 0)
	{
		pCtrl->motoPos -= pCtrl->fullStepAng;
	}

	pos = (int)(pCtrl->motoPos + 0.5f*(pCtrl->motoPos>=0?1:-1));

	if (pos > MAX_MOTO_POS)
	{
		pos -= MAX_MOTO_POS;
		pCtrl->motoPos -= MAX_MOTO_POS;
	}

	if (pos < -MAX_MOTO_POS)
	{
		pos += MAX_MOTO_POS;
		pCtrl->motoPos += MAX_MOTO_POS;
	}

	StepMotoOut(motoIdx, pos, eff);

	return pos;
}

//-------------------------------------------------------------------------------------

// 电机走一个微步函数
// dir 方向。 1,  正向; -1, 反向; 0 不走
// eff 电流值
// 返回电机的位置

int MotoOneMicoStep(int motoIdx, int dir, int eff)
{
	int pos;

	StepMotoCtrl * pCtrl;
	if (motoIdx < 0 || motoIdx >= STEP_MOTOS_NUM)
	{
		return 0;
	}
	pCtrl = &(g_stepMotoCtrl[motoIdx]);

	if (dir > 0)
	{
		pCtrl->motoPos += pCtrl->micoStepAng;
	}
	else if (dir < 0)
	{
		pCtrl->motoPos -= pCtrl->micoStepAng;
	}

	pos = (int)(pCtrl->motoPos + (0.5f*(pCtrl->motoPos>=0?1:-1)));

	if (pos > MAX_MOTO_POS)
	{
		pos -= MAX_MOTO_POS;
		pCtrl->motoPos -= MAX_MOTO_POS;
	}

	if (pos < -MAX_MOTO_POS)
	{
		pos += MAX_MOTO_POS;
		pCtrl->motoPos += MAX_MOTO_POS;
	}

	StepMotoOut(motoIdx, pos, eff);

	return pos;
}