/*
 * MISC.c
 *
 *  Created on: Dec 15, 2020
 *      Author: user
 */

#include "globals.h"
#include "main.h"
#include "MISC.h"
#include <stdio.h>
#include <stdarg.h>

//#include "menu.h"


uint8_t swapBitsInByte(uint8_t x, uint8_t p1,uint8_t p2, uint8_t n)
{
    /* Move all bits of first set to rightmost side */
	uint8_t set1 = (x >> p1) & ((1U << n) - 1);

    /* Move all bits of second set to rightmost side */
	uint8_t set2 = (x >> p2) & ((1U << n) - 1);

    /* Xor the two sets */
	uint8_t Xor = (set1 ^ set2);

    /* Put the Xor bits back to their original positions */
    Xor = (Xor << p1) | (Xor << p2);

    /* Xor the 'Xor' with the original number so that the
    two sets are swapped */
    uint8_t result = x ^ Xor;

    return result;
}

//uint16_t CRC16HW_modbus(char* buf , int len)
//{
//	//CRC->CR = 0xa8;
//	//CRC->POL = 0x8005;
//	uint16_t crc = HAL_CRC_Calculate(&hcrc, (uint32_t *)buf, len);
//	crc = (crc>>8) + (crc<<8);
//	return(crc);
//}

uint16_t CRC16SW_modbus(char * buf , int len)
{
	  uint16_t crc = 0xFFFF;
	  for (int pos = 0; pos < len; pos++) {

	    crc ^= (uint16_t)buf[pos];          // XOR byte into least sig. byte of crc

	    for (int i = 8; i != 0; i--) {    // Loop over each bit
	      if ((crc & 0x0001) != 0) {      // If the LSB is set
	        crc >>= 1;                    // Shift right and XOR 0xA001
	        crc ^= 0xa001;
	      }
	      else                            // Else LSB is not set
	        crc >>= 1;                    // Just shift right
	    }
	  }
	  // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
	  crc = (crc>>8) + (crc<<8);
	  return crc;

 }


#define IWDG_T04 {IWDG->KR=0xCCCC; IWDG->KR=0x5555; IWDG->PR=3; HAL_Delay(10);IWDG->KR=0xAAAA;}
#define IWDG_T02 {IWDG->KR=0xCCCC; IWDG->KR=0x5555; IWDG->PR=2; HAL_Delay(10);IWDG->KR=0xAAAA;}
#define IWDG_T32 {IWDG->KR=0xCCCC; IWDG->KR=0x5555; IWDG->PR=7; HAL_Delay(10);IWDG->KR=0xAAAA;}
#define IWDG_RES {IWDG->KR=0xAAAA;}









int FPRINTF(UART_HandleTypeDef *huart, const char *str, ...)
{
	va_list arg;
	char BUF[500];
	va_start(arg,str);
	vsprintf(BUF, str, arg);
	//__disable_irq();
	HAL_UART_Transmit(huart,(uint8_t*)BUF,strlen(BUF),1000);
	//__enable_irq();
	va_end(arg);
	return(strlen(BUF));
}

int FSCANF(UART_HandleTypeDef *huart,uint8_t *buf)
{
	uint8_t *p;
	uint8_t a=0;
	p = buf;
	do
	{
		if(HAL_UART_Receive(huart,&a,1,60000)==HAL_TIMEOUT)
		{
  			FPRINTF(DEB,"\r\nMINAL CZAS\r\n");
  			*p = 0;
  			return(strlen((char*)p));
		}
		if((a=='\n') || (a=='\r'))
		{
			*(p) = 0;
			return(strlen((char*)p));
		}
		*(p++) = a;
	}while(1);
}



