#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include <util/delay.h>
//#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#include <stdint.h>
#include <math.h>



/*
 H       ;   T     ;  O          ;   B
  -        ;   -      ;  -            ;   1010 0010  ;  162
  -        ;   -      ;   xxx1    ;   010   0010  ;  <<#1
  -        ;   -      ;   xx10     ;   10   0010    ;  <<#2
  -        ;   -      ;   x101    ;  0   0010       ;  <<#3
  -        ;   -      ;   1000    ;                      ; add 3
  -        ;  xxx1 ;   0000    ;   0010           ;  <<#4
  -        ;  xx10   ;   0000    ;   010             ;<<#5
 -        ;   x100   ;   0000    ;   10               ;<<#6
 -        ;   x1000   ;  0001    ;   0                ;<<#7
 -        ;   1011   ;               ;                      ; add 3
 1       ;    0110   ;   0010  ;                       ;<<#8           
 1      ;     6         ;   2        ; 
*/
 
 
 
 uint8_t adjBCD(uint8_t r)
{
uint8_t tmp=r+0x03;
if(tmp&0x08) { r=tmp; }  // if (r+0x03)>7 (bit 3==1) r+=0x03 
tmp=r+0x30;
if(tmp&0x80 ) {r =tmp ; } //bit7 , for packed if in the (r+0x03<<4 ) (bit 7==1) r+=0x03<<4 ; 
return r;   
}
 
 uint8_t * unpackBCD(uint8_t * arg)
{


uint8_t  temp[6];
temp[5]=arg[0]&0x0F;      // LSB
temp[4]=(arg[0]&0xF0)>>4;
temp[3]=arg[1]&0x0F;
temp[2]=(arg[1]&0xF0)>>4;
temp[1]=arg[2]&0x0F;
temp[0]=(arg[2]&0xF0)>>4; //MSB

return (uint8_t *) temp;
} 

 
uint8_t *  BinToBCD(uint32_t x  )
{
   // using modifyed    AN526 
 uint8_t count =0; 
 uint8_t Res[3]; 
   Res[0]=0;  
   Res[1]=0;
   Res[2]=0;
  
   
//clear Carry and use shift ing of x or load MSB of x to Carry without shifting, then use shifting after 
    
// Successive rotation   x<16777215   
//for (count=24 /*16*/; count>0 ; count--)  //total 16 steps 
// Successive rotation   x<262143
//for (count=18 ; count>0 ; count--) 
// Successive rotation   x<131071

for (count=17 ; count>0 ; count--)   
{
// add 3 for columns >=5 
 /*
if(Th>=5) { Th+=3; } 
if(H>=5) { H+=3; }   
if(T>=5) { T+=3; } 
if(O>=5) { O+=3; } 
 
Th=adjBCD(Th);  
H=adjBCD(H); 
T=adjBCD(T);  
O=adjBCD(O);
 
  Th=0x0F&(Th<<1)| ((H>>3)&0x01);   //Th=0x0F&((Th<<1)|(H.bit3)
  H=0x0F&(H<<1)| ((T>>3)&0x01);     //H=0x0F&((H<<1)|(T.bit3) 
  T=0x0F&(T<<1)| ((O>>3)&0x01);     //T=0x0F&((T<<1)|(O.bit3) 
  O=(uint8_t)0x0F&(O<<1)|((x>>15-i )&0x01);    //  MSB to O.bit0
//x=x<<1;  (shift left once  to load data into carry )
  */ 
      Res[2]=adjBCD(Res[2]);
      Res[1]=adjBCD(Res[1]);
      Res[0]=adjBCD(Res[0]);
     

  // R=R<<1;
  // R|=(uint32_t)((x>>count-1)&0x0001)
 
     Res[2]=(uint8_t) (Res[2]<<1)|((Res[1]>>7)&0x01);   //rlf r2
     Res[1]=(uint8_t) (Res[1]<<1)|((Res[0]>>7)&0x01);  // rlf r1,  R1= (H<<1)| ((R0>>3)&0x01); 
     Res[0]=(uint8_t) (Res[0]<<1) ; //  shift left Res     
     Res[0]|= (uint8_t) ((uint8_t) (x>>(count-1)) &   0x01);

}
 
 return (uint8_t *)  unpackBCD((uint8_t *) Res);
 //return   Res ; // RL
 
} 
 
 

 