#include "stdint.h"
#include "V2RFEncoding.h"
#define V2_OFFSET_JUMP_START    0x54
#define V2_PACKET_LEN           9

#define V2_OFFSET(byte, key, jumpStart) ( \
  V2_OFFSETS[byte-1][key%4] \
    + \
  ((jumpStart > 0 && key >= jumpStart && key < jumpStart+0x80) ? 0x80 : 0) \
)

uint8_t const V2_OFFSETS[][4] = {
  { 0x45, 0x1F, 0x14, 0x5C }, // request type
  { 0x2B, 0xC9, 0xE3, 0x11 }, // id 1
  { 0x6D, 0x5F, 0x8A, 0x2B }, // id 2
  { 0xAF, 0x03, 0x1D, 0xF3 }, // command
  { 0x1A, 0xE2, 0xF0, 0xD1 }, // argument
  { 0x04, 0xD8, 0x71, 0x42 }, // sequence
  { 0xAF, 0x04, 0xDD, 0x07 }, // group
  { 0x61, 0x13, 0x38, 0x64 }  // checksum
};

uint8_t xorKey(uint8_t key) {
  // Generate most significant nibble
  const uint8_t shift = (key & 0x0F) < 0x04 ? 0 : 1;
  const uint8_t x = (((key & 0xF0) >> 4) + shift + 6) % 8;
  const uint8_t msn = (((4 + x) ^ 1) & 0x0F) << 4;

  // Generate least significant nibble
  const uint8_t lsn = ((((key & 0xF) + 4)^2) & 0x0F);

  return ( msn | lsn );
}

uint8_t decodeByte(uint8_t byte, uint8_t s1, uint8_t xorKey, uint8_t s2) {
  uint8_t value = byte - s2;
  value = value ^ xorKey;
  value = value - s1;

  return value;
}

uint8_t encodeByte(uint8_t byte, uint8_t s1, uint8_t xorKey, uint8_t s2) {
  uint8_t value = byte + s1;
  value = value ^ xorKey;
  value = value + s2;

  return value;
}

void decodeV2Packet(uint8_t *packet) {
  uint8_t key = xorKey(packet[0]);

  for (uint8_t i = 1; i <= 8; i++) {
    packet[i] = decodeByte(packet[i], 0, key, V2_OFFSET(i, packet[0], V2_OFFSET_JUMP_START));
  }
}

void encodeV2Packet(uint8_t *packet) {
  uint8_t key = xorKey(packet[0]);
  uint8_t sum = key;

  for (uint8_t i = 1; i <= 7; i++) {
    sum += packet[i];
    packet[i] = encodeByte(packet[i], 0, key, V2_OFFSET(i, packet[0], V2_OFFSET_JUMP_START));
  }

  packet[8] = encodeByte(sum, 2, key, V2_OFFSET(8, packet[0], 0));
}

uint8_t defaultPackage[V2_PACKET_LEN] ={00,0xD8,0xDD,0xD1,0xE6,0xD3,0x3A,0x5F,0xE0 };

void TestDecode(void)
{
    uint8_t Package[V2_PACKET_LEN];
    uint8_t Package2[V2_PACKET_LEN];
    
    memcpy(Package, defaultPackage, V2_PACKET_LEN);
    decodeV2Packet(Package);
    
    memcpy(Package2, Package, V2_PACKET_LEN);
    encodeV2Packet(Package2);
}
/*
fut089 packet received (9 bytes):
Raw packet: 00 D8 DD D1 E6 D3 3A 5F E0 

Decoded:
Key      : 00    //密码
b1       : 25    //灯类型
ID       : 04D2  //遥控器ID
Command  : 81    //控制命令
Argument : 0F    //参数
Sequence : 80    //序列
Group    : 06    //灯组
Checksum : C9    //校验
*/