#include "IDIBUS_4CNTtoPWM.h"

IdiBus_4CNTtoPWM::IdiBus_4CNTtoPWM (IdiBusSerialLine *SlaveLine, uint8_t SlaveAddress) : IdiBusSlave(SlaveLine, SlaveAddress)
{
	for (uint8_t I=0; I<IDIBUS_4CNTtoPWM_CH_COUNT; I++)
	{
		this->CH[I].MMES_Error = 0;
		this->CH[I].PWM = 0;
		//this->CH[I].STATUS.Enable = 0;
	}
}

IdiBus_4CNTtoPWM::IdiBus_4CNTtoPWM () : IdiBusSlave()
{
	for (uint8_t I=0; I<IDIBUS_4CNTtoPWM_CH_COUNT; I++)
	{
		this->CH[I].MMES_Error = 0;
		this->CH[I].PWM = 0;
		//this->CH[I].STATUS.Enable = 0;
	}
}

IDIBUS_4CNTtoPWM_DATA_TYPE IdiBus_4CNTtoPWM::getChData(uint8_t ChNum){
	if ( ChNum < IDIBUS_4CNTtoPWM_CH_COUNT ) { return this->CH[ChNum]; }
	else								{ return this->CH[IDIBUS_4CNTtoPWM_CH4_NUM]; }
}

uint8_t IdiBus_4CNTtoPWM::setChPwm(uint8_t ChNum, uint8_t pwm)
{
	if (ChNum >= IDIBUS_4CNTtoPWM_CH_COUNT) { return IDIERMST_INVALID_CHN_NUM; }

	uint8_t TxData[1];
	TxData[0] = pwm;

	IdiBusMMES MMES(this->Address, &TxData[0], 1);
	MMES.DEV.NUM	= IDIBUS_4CNTtoPWM_CNTRL_DEV;
	MMES.CHNL.NUM   = ChNum;
	MMES.ComFunc    = 0x02;		//Set Value

	uint8_t RequestError = this->SendRequestMMES(&MMES);

	if      ( RequestError   != IDIER_NOPE )          { this->CH[ChNum].MMES_Error = RequestError;   }
	else if ( MMES.RxDataLength != 1 )                { this->CH[ChNum].MMES_Error = IDIERMST_INVALID_RX_REQUEST_FORMAT; }
	else {
		this->CH[ChNum].MMES_Error = MMES.RxData[0];
		if ( this->CH[ChNum].MMES_Error == IDIER_NOPE )
		{
			this->CH[ChNum].PWM = pwm;
		}
	}
	return this->CH[ChNum].MMES_Error;

}

void IdiBus_4CNTtoPWM::setAllChsPwm(void)
{
 uint8_t  TxData[IDIBUS_4CNTtoPWM_CH_COUNT];

 for (uint8_t I=0; I<IDIBUS_4CNTtoPWM_CH_COUNT; I++)
	{
	 TxData[I]   = this->CH[I].PWM;
    }

 IdiBusMMES MMES(this->Address, &TxData[0], IDIBUS_4CNTtoPWM_CH_COUNT);
 MMES.DEV.NUM       = IDIBUS_4CNTtoPWM_CNTRL_DEV;
 MMES.DEV.ALLCH     = 1;
 MMES.ComFunc       = 0x02;
 uint8_t RequestError = this->SendRequestMMES(&MMES);
 if      ( RequestError != IDIER_NOPE )             { this->setAllChErr(RequestError); }
 else if (MMES.RxDataLength != IDIBUS_4CNTtoPWM_CH_COUNT) { this->setAllChErr(IDIERMST_INVALID_RX_REQUEST_FORMAT); }
 else
	{
	 for (uint8_t I = 0; I < IDIBUS_4CNTtoPWM_CH_COUNT; I++)
		{
		 this->CH[I].MMES_Error = MMES.RxData[I];
		 //if ( this->CH[I].MMES_Error == IDIER_NOPE )  { this->CH[I].TargetVoltage = TargetV[I]; }
		}
	}
}

/*
void IdiBus_4CNTtoPWM::setAllChsCounts(void)
{
	IdiBusMMES MMES(this->Address);
	MMES.DEV.NUM       = IDIBUS_4CNTtoPWM_CNTRL_DEV;
	MMES.DEV.ALLCH     = 1;
	MMES.ComFunc       = 0x02;
	uint8_t ChDataSize = 4;		//uint32_t
	uint8_t RequestError = this->SendRequestMMES(&MMES);
	if      ( RequestError != IDIER_NOPE )            { this->setAllChErr(RequestError); return; }
	else if (MMES.RxDataLength < IDIBUS_4CNTtoPWM_CH_COUNT) { this->setAllChErr(IDIERMST_INVALID_RX_REQUEST_FORMAT); return; }
	else {
		uint8_t ExpDataLength = IDIBUS_4CNTtoPWM_CH_COUNT; // Errors
		for (uint8_t I=0; I<IDIBUS_4CNTtoPWM_CH_COUNT; I++) {  if ( MMES.RxData[I] == IDIER_NOPE ) { ExpDataLength = (uint16_t)(ExpDataLength + ChDataSize); }  }
		if ( ExpDataLength != MMES.RxDataLength )     {  this->setAllChErr(IDIERMST_INVALID_RX_REQUEST_FORMAT); return;  }
	}
	uint8_t ErrorPos = 0;
	uint8_t DataPos  = ErrorPos + IDIBUS_4CNTtoPWM_CH_COUNT;
	for (uint8_t I = 0; I < IDIBUS_4CNTtoPWM_CH_COUNT; I++)
	{
		this->CH[I].MMES_Error = MMES.RxData[ErrorPos];
		ErrorPos = (uint8_t)(ErrorPos + 1);
		if ( this->CH[I].MMES_Error == IDIER_NOPE ) {
			this->CH[I].Counter = (uint32_t) (
			((uint32_t)MMES.RxData[DataPos]	  << 24)  |
			((uint32_t)MMES.RxData[DataPos+1] << 16)  |
			((uint32_t)MMES.RxData[DataPos+2] << 8 )  |
			((uint32_t)MMES.RxData[DataPos+3])      );
			DataPos  = (uint8_t)(DataPos  + ChDataSize);
		}
	}
}
*/

void IdiBus_4CNTtoPWM::setAllChErr(uint8_t ErrCode)
{
	for (uint8_t I=0; I<IDIBUS_4CNTtoPWM_CH_COUNT; I++) { this->CH[I].MMES_Error = ErrCode; }
}

uint8_t IdiBus_4CNTtoPWM::initCh(uint8_t ChNum)
{
	if (ChNum >= IDIBUS_4CNTtoPWM_CH_COUNT) { return IDIERMST_INVALID_CHN_NUM; }
	IdiBusMMES MMES(this->Address);
	MMES.DEV.NUM       = IDIBUS_4CNTtoPWM_CNTRL_DEV;
	MMES.CHNL.NUM	   = ChNum;
	MMES.ComFunc       = 0x01; //init
	uint8_t RequestError = this->SendRequestMMES(&MMES);
	if      ( RequestError   != IDIER_NOPE )          { this->CH[ChNum].MMES_Error = RequestError;   }
	else if ( MMES.RxDataLength != 1 )                { this->CH[ChNum].MMES_Error = IDIERMST_INVALID_RX_REQUEST_FORMAT; }
	else {
		this->CH[ChNum].MMES_Error = MMES.RxData[0];
	}
	return this->CH[ChNum].MMES_Error;

}

void IdiBus_4CNTtoPWM::initAllChs(void)
{
	//uint8_t TxData[2];
	//TxData[0] = (uint8_t)(CurrentLimit >> 8);
	//TxData[1] = (uint8_t)(CurrentLimit);
	IdiBusMMES MMES(this->Address);
	MMES.DEV.NUM       = IDIBUS_4CNTtoPWM_CNTRL_DEV;
	MMES.DEV.ALLCH	   = 1;
	MMES.ComFunc       = 0x01;	//init
	uint8_t RequestError = this->SendRequestMMES(&MMES);
	if      ( RequestError != IDIER_NOPE )             { this->setAllChErr(RequestError); }
	else if (MMES.RxDataLength != IDIBUS_4CNTtoPWM_CH_COUNT) { this->setAllChErr(IDIERMST_INVALID_RX_REQUEST_FORMAT); }
	else
	{
		for (uint8_t I = 0; I < IDIBUS_4CNTtoPWM_CH_COUNT; I++)
		{
			this->CH[I].MMES_Error = MMES.RxData[I];
			//if ( this->CH[I].MMES_Error == IDIER_NOPE )
		}
	}
}

