#include <stdio.h>
#include <string.h>

#define SHORTEN_PREAMBLE_TO_32 0

typedef unsigned char byte;
typedef unsigned short uint16;

uint16 crc(byte *data, int len) 
{
    byte i, x, crcdata;
    uint16 crcReg = 0xFFFF;  
    for (x = 0; x < len; x++)
    { 
        crcdata = data[x];
        for (i = 0; i < 8; i++) 
        {
            if (((crcReg & 0x8000) >> 8) ^ (crcdata & 0x80))
                crcReg = (crcReg << 1) ^ 0x8005;
            else
                crcReg = (crcReg << 1);        
            crcdata <<= 1;
        }
    }
    return crcReg;
}

byte str2byte(char str[8])
{
    int ret = 0, i;
    for(i = 0; i < 8; i++)
        if (str[i]=='1') ret |= (1<<(7-i));
    return ret;
}

void print_binary(byte inpt)
{
    int i;
    for(i = 0; i < 8; i++)
        if (inpt & (1<<(7-i))) putchar('1');
        else putchar('0');
}

void print_preamble_nibbles(int count)
{
    int i;
    for (i = 0; i < count; i++)
        printf("1010");
}

int find_preamble_start_in_bit(char *string, int len)
{
    char homematic_sync[] = "11101001110010101110100111001010";    
    for(int i = 0, j = 0; i < len; i++)
    {
        if(string[i] == homematic_sync[j])
        {     
            j++;
            if(j == 32 && i>= 63) return i-63;            
        }
        else j = 0;
    }
    return -1; //not found
}

void xor_lfsr(char *string)
{
    int i, j, x, len = strlen(string);
    byte polynomial[9] = {0, 0, 0, 1, 0, 0, 0, 0, 1};
    byte lfsr_state[9];
    byte first_bit;
    
    // Init with 8x 1 Bit
    memset(lfsr_state, 1, 9);
    for(j = 0; j < 8; j++)
        if(string[j]=='1')
            string[j]='0';                
        else
            string[j]='1';            
        
    for(x = 8; x < len-7; x += 8)
    {        
        for(i = 0; i < 8; i++)
        {        
            first_bit = 255;
            for(j = 8; j >= 0; j--)
                if(polynomial[j] && lfsr_state[j])
                {
                    if(first_bit == 255)
                        first_bit = 1;
                    else
                        first_bit = (first_bit==1) ? 0 : 1;                
                }
            if(first_bit == 255)
                first_bit = 0;
            
            // Clock
            for(j = 8; j >= 0; j--)
                lfsr_state[j] = lfsr_state[j-1];
            lfsr_state[0] = first_bit;
        }

        // Xor
        for(j = 0; j < 8; j++)
            if(lfsr_state[j+1] == 1)
                if(string[x+j]=='1')
                    string[x+j]='0';                
                else
                    string[x+j]='1';
    }
}

int main(int argc, char **argv)
{
    int i, j, max, offset, len, preamble_additional_length;
    byte dec[1024]={0}, enc[1024]={0}, crc_ok;
    char string[65536]={0};
    uint16 crcvalue;
    offset = 8; // Preamble + Sync
    
    // Copy data (argv[2]) to string if length is ok, shorten to multiple of 8 bit
    if (strlen(argv[2]) > 8192*8 || strlen(argv[2]) < 4) return -1;
    len = strlen(argv[2]);    
    
    i = find_preamble_start_in_bit(argv[2], len);
    if(i < 0) return 0;    // preamble+sync not found or wrong length
    preamble_additional_length = i;
    
    len = (len-i)-(len-i)%8;
    memcpy(string, argv[2]+i, len);
               
    if (argc>2)
    {
        if (argv[1][0]=='d')
        {   
            // Apply datawhitening      
            xor_lfsr(string+64);
                               
            // Pack to bytes
            for (i = 0; i < strlen(string)-3; i+=8)
                enc[i/8] = str2byte(&string[i]);                
            max = i/8;
            memcpy(&dec, &enc, 1024);

            // Check CRC
            crcvalue = crc(&dec[8], max-2-8);
            crc_ok = 0;
            if( ((crcvalue >> 8) & 0xFF) == dec[max-2] && (crcvalue & 0xFF) == dec[max-1]) crc_ok = 1;
            
            /*
             * byte[] Dec = new byte[Enc.Length];
             * Dec[0] = Enc[0]; //Packet length
             * Dec[1] = (byte)((~Enc[1]) ^ 0x89);
             * int j;
             * for (j = 2; j < Dec[0]; j++)
             *   Dec[j] = (byte)((Enc[j-1] + 0xdc) ^ Enc[j]);
             * Dec[j] = (byte)(Enc[j] ^ Dec[2]);
             */
            
            // Decrypt
            dec[offset+0] = enc[offset+0];            
            dec[offset+1] = (~enc[offset+1])^0x89;            
            for(i = offset + 2; i < max - 3; i++)
                dec[i] = (enc[i-1]+0xdc) ^ enc[i];
            dec[i] = enc[i] ^ dec[offset+2];
            
            // Recompute CRC and overwrite with FAKE-CRC, if CRC was OK before
            if(crc_ok)
            {
                crcvalue = crc(&dec[8], max-2-8);
                dec[max-1] = crcvalue & 0xFF; 
                dec[max-2] = (crcvalue >> 8) & 0xFF;
            }
            else
            {
                dec[max-1] = 0x0F; // Set magic code for wrong CRC
                dec[max-2] = 0xD0;
            }
            
            // Prepend preamble longer than 32 bits
            if(0 == SHORTEN_PREAMBLE_TO_32 && preamble_additional_length > 0) 
                print_preamble_nibbles(preamble_additional_length/4);
                
            for(i = 0; i < max; i++)
                print_binary(dec[i]);
        }
        else
        {
            // Pack to bytes
            for (i = 0; i < strlen(string)-3; i+=8)
                dec[i/8] = str2byte(&string[i]);
            max = i/8;
            memcpy(&enc, &dec, 1024);
            
            /*
             * byte[] Dec = new byte[Enc.Length];
             * Dec[0] = Enc[0]; //Packet length
             * Dec[1] = (byte)((~Enc[1]) ^ 0x89);
             * int j;
             * for (j = 2; j < Dec[0]; j++)
             *   Dec[j] = (byte)((Enc[j-1] + 0xdc) ^ Enc[j]);
             * Dec[j] = (byte)(Enc[j] ^ Dec[2]);
             */
            
            // Encrypt
            enc[offset+0] = dec[offset+0];            
            enc[offset+1] = ~(dec[offset+1])^0x89;            
            for(i = offset + 2; i < max - 3; i++)
                enc[i] = (enc[i-1]+0xdc) ^ dec[i];
            enc[i] = dec[i] ^ dec[offset+2];
            
            // Overwrite with correct CRC
            crcvalue = crc(&enc[8], max-2-8);
            enc[max-1] = crcvalue & 0xFF;
            enc[max-2] = (crcvalue >> 8) & 0xFF;            
            
            // Convert to string
            memset(string, 0, 65536);
            for(i = 0; i < max; i++)
            {
                for(j = 0; j < 8; j++)
                    if(enc[i] & (1<<(7-j))) string[i*8+j]='1';
                    else string[i*8+j]='0';
            }
            // Apply datawhitening
            xor_lfsr(string+64);
            
            // Prepend preamble longer than 32 bits
            if(0 == SHORTEN_PREAMBLE_TO_32 && preamble_additional_length > 0) // Add preamble longer than 32 bits
                print_preamble_nibbles(preamble_additional_length/4);
            
            // Print bits and duplicate last bit
            printf("%s%c\n", string, string[strlen(string)-1]);
        }
    }
    else printf("Usage: %s <d/e> <bit sequence>\n\td - decode\n\te - encode\n\tbit sequence as string of 0 and 1.\n", argv[0]);
    
    return 0;
}