#include <CRC16.h>

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

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

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 setSourceRemoteAddress();
void setHubSourceAddress();
void setRollingCodes();
void setProtocolVersion(uint8_t);
void calculateCRC();


void constructUpPacket()
{
  setPacketLength(LENGTH_17);
  setConstants();
  setSourceRemoteAddress();
  setRollingCodes();
  setProtocolVersion(VERSION_6);

  buf[11] = 0x04; // Shade ID (group 4 for now)
  buf[12] = 0x00; // Shade ID

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

  calculateCRC();
}

void constructDownPacket()
{
  setPacketLength(LENGTH_17);
  setConstants();
  setSourceRemoteAddress();
  setRollingCodes();
  setProtocolVersion(VERSION_6);

  buf[11] = 0x04; // Shade ID (group 4 for now)
  buf[12] = 0x00; // Shade ID
  
  buf[16] = 0x44; // Command
  buf[17] = 0x00; // Empty?

  calculateCRC();
}

void constructStopPacket()
{
  setPacketLength(LENGTH_17);
  setConstants();
  setSourceRemoteAddress();
  setRollingCodes();
  setProtocolVersion(VERSION_6);

  buf[11] = 0x04; // Shade ID (group 4 for now)
  buf[12] = 0x00; // Shade ID

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

  calculateCRC();
}

void constructPositionPacket(float percentage)
{
  setPacketLength(LENGTH_21);
  setConstants();
  setHubSourceAddress(); // TODO: Will this source address work?
  setRollingCodes();
  setProtocolVersion(VERSION_5);

  buf[11] = 0x4E; // Shade ID?
  buf[12] = 0xF1; // Shade ID?

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

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

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

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

  calculateCRC();
}

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

void setConstants() {
  buf[1] = 0x00;  // Constant?
  buf[2] = 0x05;  // Constant?

  buf[4] = 0xff;  // Constant?
  buf[5] = 0xff;  // Constant?

  buf[8] = 0x86;  // Constant?

  buf[15] = 0x52; // Constant?
}

void setSourceRemoteAddress() {
  buf[6] = 0x36;  // Source address?
  buf[7] = 0x9e; // Source address?

  buf[13] = 0x36; // Source address again?
  buf[14] = 0x9e; // Source address again?
}

void setHubSourceAddress() {
  buf[6] = 0x00;  // Source address?
  buf[7] = 0x00; // Source address?

  buf[13] = 0x00; // Source address again?
  buf[14] = 0x00; // Source address again?
}

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

void setProtocolVersion(uint8_t version) {
  buf[9] = 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 + 1] = checksum1; // Checksum
  buf[length + 2] = 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();
}