#include "ibus.h"

uint16_t CRC16Gen(IBusFrame frame){
	uint16_t checksum = 0x0;
	for (int i = 0; i < frame[0]-2; i++) {
		checksum += frame[i];
	}
	checksum = 0xFFFF - checksum;

	return checksum;
}

int encodeIBus(IBusFrame frame, channelValues vals){
	*frame=IBusSize;
	*(frame+1)=startByteBeta;
	for (int i=0; i<channelC*2; i++){
		if (i%2)
			*(frame+i+2)=(uint8_t) (*(vals+(i/2)))>>sizeof(uint8_t);
		else
			*(frame+i+2)=(uint8_t) (*(vals+(i/2)) & 0xFF);
	}
	uint16_t checksum=CRC16Gen(frame);
	*(frame+frame[0]-2)=(uint8_t)(checksum & 0xFF);
	*(frame+frame[0]-1)=(uint8_t)(checksum>>8);
	
}