#include <CRC16.h>

#define LENGTH_17                      (0x11)
#define LENGTH_21                      (0x15)
#define LENGTH_25                      (0x19)

#define VERSION_5                      (0x05)
#define VERSION_6                      (0x06)

#define REMOTE_ID                      (0x369E)
#define HUB_ID                         (0x0000)

uint8_t rollingCode1 = 0xAC;
uint8_t rollingCode2 = 0x82;

uint8_t buf[32];

CRC16 crc(0x755B, 0x0, 0xBA68, false, false);

void setPacketLength(uint8_t);
void setConstants();
void setSourceAddress(uint16_t);
void setTargetAddress(uint16_t);
void setRollingCodes();
void setProtocolVersion(uint8_t);
void calculateCRC();


void constructUpPacket(uint16_t shadeID)
{
  setPacketLength(LENGTH_17);
  setConstants();
  setSourceAddress(REMOTE_ID);
  setTargetAddress(shadeID);
  setRollingCodes();
  setProtocolVersion(VERSION_6);

  buf[16] = 0x52; // ?
  buf[17] = 0x55; // Command
  buf[18] = 0x00; // Empty?

  calculateCRC();
}

void constructDownPacket(uint16_t shadeID)
{
  setPacketLength(LENGTH_17);
  setConstants();
  setSourceAddress(REMOTE_ID);
  setTargetAddress(shadeID);
  setRollingCodes();
  setProtocolVersion(VERSION_6);

  buf[16] = 0x52; // ?
  buf[17] = 0x44; // Command
  buf[18] = 0x00; // Empty?

  calculateCRC();
}

void constructStopPacket(uint16_t shadeID)
{
  setPacketLength(LENGTH_17);
  setConstants();
  setSourceAddress(REMOTE_ID);
  setTargetAddress(shadeID);
  setRollingCodes();
  setProtocolVersion(VERSION_6);

  buf[16] = 0x52; // ?
  buf[17] = 0x53; // Command
  buf[18] = 0x00; // Empty?

  calculateCRC();
}

void constructPositionPacket(uint16_t shadeID, float percentage)
{
  setPacketLength(LENGTH_21);
  setConstants();
  setSourceAddress(HUB_ID);
  setTargetAddress(shadeID);
  setRollingCodes();
  setProtocolVersion(VERSION_5);

  buf[16] = 0x3F; // Command
  buf[17] = 0x5A; // Empty?

  buf[18] = 0x04; // Constant in this size packet?
  buf[19] = 0x40; // Constant in this size packet?
  buf[20] = 0x50; // Constant in this size packet?

  uint16_t position = (uint16_t)(0xFFFF * percentage);

  buf[21] = (uint8_t)(position & 0x00FF);
  buf[22] = (uint8_t)((position & 0xFF00) >> 8);

  calculateCRC();
}

void setPacketLength(uint8_t length) {
  buf[1] = length;  // Packet size
}

void setConstants() {
  buf[0] = 0xC0;  // Header byte

  buf[2] = 0x00;  // Constant when sending, can be 0x10 when receiving a packet
  buf[3] = 0x05;  // Constant

  buf[5] = 0xFF;  // Constant
  buf[6] = 0xFF;  // Constant

  buf[9] = 0x86;  // Constant?
}

void setSourceAddress(uint16_t sourceID) {
  // Physical source address (could be the address of a repeater when receiving a packet)
  buf[7] = (uint8_t)((sourceID & 0xFF00) >> 8);
  buf[8] = (uint8_t)(sourceID & 0x00FF);

  // Logical source address (usually the same as the physical source address)
  buf[14] = (uint8_t)((sourceID & 0xFF00) >> 8);
  buf[15] = (uint8_t)(sourceID & 0x00FF);
}

void setTargetAddress(uint16_t targetID) {
  // Logical target address
  buf[12] = (uint8_t)((targetID & 0xFF00) >> 8);
  buf[13] = (uint8_t)(targetID & 0x00FF);
}

void setRollingCodes() {
  buf[4] = rollingCode1;  // Rolling code 1
  buf[11] = rollingCode2; // Rolling code 2
}

void setProtocolVersion(uint8_t version) {
  buf[10] = version; // Protocol version?
}

void calculateCRC() { // must be called after the buffer has been filled
  crc.restart();
  uint8_t length = buf[0];
  switch(length) {
    case 0x11:
      crc.setXorOut(0xBA68);
      break;
    case 0x15:
      crc.setXorOut(0xE10C);
      break;
    case 0x19:
      crc.setXorOut(0x89A0);
      break;
    default:
      Serial.print("Unsupported length (");
      Serial.print(length);
      Serial.println("). Cannot calculate CRC");
      return;
  }
  crc.add(buf, length + 1);
  uint16_t result = crc.calc();
  uint8_t checksum1 = (uint8_t)((result & 0xFF00) >> 8);
  uint8_t checksum2 = (uint8_t)(result & 0x00FF);

  buf[length + 2] = checksum1; // Checksum
  buf[length + 3] = checksum2; // Checksum
}

void printHex(uint8_t num) {  //print a byte as two hex chars
  char hexCar[3];
  sprintf(hexCar, "%02X", num);
  Serial.print(hexCar);
}

void printBuffer() //print a byte array as hex chars
{
  uint8_t bytecount = buf[0] + 3;
  for (int i = 0; i < bytecount; i++) {
    printHex(buf[i]);
  }
  Serial.println();
}