#include <SPI.h>;
#include <mcp2515.h>;

struct can_frame _frame;
MCP2515 mcp2515(10);

bool flag = false;
byte DeviceId[2];

void setup() {
  Serial.begin(9600);
  SPI.begin();

  mcp2515.reset();
  mcp2515.setBitrate(CAN_500KBPS, MCP_8MHZ); //Sets CAN at speed 500KBPS and Clock 8MHz
  mcp2515.setNormalMode();

  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(A3, OUTPUT);

}

int i = 0;
bool debugState = false;

void loop() {

  int analogValue = analogRead(A3);
  if (analogValue > 756 && !debugState)
  {
    debugState = true;
    SendSwitchedTriggeredCanPackage(5, debugState);
  }
  if (analogValue < 300 && debugState)
  {
    debugState = false;
    SendSwitchedTriggeredCanPackage(5, debugState);

  }

  if (mcp2515.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    int meyPinId = _frame.data[0];
    int state = _frame.data[1] > 0;
    Serial.println("CAN FRAME ------------");
    Serial.print("0x");
    Serial.println( _frame.can_id ^ CAN_EFF_FLAG, HEX);

    Serial.print("PackageType: ");
    Serial.println(GetPackageType(_frame.can_id), HEX);
    
    Serial.print("DeviceId: ");
    Serial.println(GetDeviceId(_frame.can_id), HEX);


    Serial.println( _frame.can_dlc);

    for (int i = 0; i < _frame.can_dlc ; i++)
    {
      Serial.println( _frame.data[i], HEX);
    }
    Serial.println("----------------------");


    if (meyPinId == 1)
    {
      digitalWrite(2, state);
    }
    if (meyPinId == 2)
    {
      digitalWrite(3, state);
    }
    if (meyPinId == 3)
    {
      digitalWrite(4, state);
    }
    if (meyPinId == 4)
    {
      digitalWrite(5, state);
    }
    if (meyPinId == 5)
    {
      digitalWrite(6, state);
    }
    if (meyPinId == 6)
    {
      digitalWrite(7, state);
    }
    if (meyPinId == 7)
    {
      digitalWrite(8, state);
    }
    if (meyPinId == 8)
    {
      digitalWrite(9, state);
    }


  }

  delay(20);
}

byte CircularShift(byte b)
{
  return (b << 1) | (b >> 7 & 1);
}

uint32_t GetDeviceId(uint32_t canFrameId)
{
  return canFrameId & 0xFFFF;
}

uint32_t GetPackageType(uint32_t canFrameId)
{
  return (canFrameId / 0x10000) & 0xFFF;
}

uint32_t CreateCanId(uint32_t commandId)
{
  return ((commandId & 0xFFF) * 0x10000) | ( DeviceId[0] << 8) | (DeviceId[1]) | CAN_EFF_FLAG;
}

const byte SoftwareVersionHigh = 1;
const byte SoftwareVersionLow = 0;
const byte HardwareVersionHigh = 2;
const byte HardwareVersionLow = 0;
const uint32_t HELP_PACKAGE_CAN_ID = 0xFFFF;
const uint32_t SWITCH_TRIGGERED_CAN_ID = 0x050;
const uint32_t TRIGGER_SWITCH_CAN_ID = 0x055;
const uint32_t  HELP_PACKAGE_ID = 0xFFFF;

void SendSerialPackage()
{
  _frame.can_id = CreateCanId(HELP_PACKAGE_CAN_ID);
  _frame.can_dlc = 4;

  _frame.data[0] = SoftwareVersionHigh;
  _frame.data[1] = SoftwareVersionLow;
  _frame.data[2] = HardwareVersionHigh;
  _frame.data[3] = HardwareVersionLow;
  mcp2515.sendMessage(MCP2515::TXB1, &_frame);
}

void SendSwitchedTriggeredCanPackage(byte pinId, int state)
{
  _frame.can_id = CreateCanId(SWITCH_TRIGGERED_CAN_ID);
  _frame.can_dlc = 2;
  _frame.data[0] = pinId;
  _frame.data[1] = state;
  mcp2515.sendMessage(MCP2515::TXB1, &_frame);
}

void SendDoTriggerSwitchCanPackage(uint32_t targetCanId, byte pinId, byte state)
{
  _frame.can_id = CreateCanId(TRIGGER_SWITCH_CAN_ID);
  _frame.can_dlc = 4;
  _frame.data[0] = targetCanId & 0xFF;
  _frame.data[1] = (targetCanId & 0xFF00) >> 8;
  _frame.data[2] = pinId;
  _frame.data[3] = state;
  mcp2515.sendMessage(MCP2515::TXB1, &_frame);
}