#include <Arduino.h>
#include <Wire.h>
const uint8_t SlaveDeviceId = 0x02;

uint16_t receivedPacket[10];

byte bytesSent = 0;
byte buffer[2];

uint16_t returninfo;

int watchDog = 0;
bool fallback = false;
unsigned long tick = 0;
int pins[18]   = {0, 0, 0, 1, 0, 2, 3, 0, 0, 4, 5, 6, 0, 0, 0, 0, 0, 0}; // map to array
int pin_map[7] = {0, 3, 5, 6, 9, 10, 11};
int target[7]  = {0, 0, 0, 0, 0, 0, 0};
int current[7] = {0, 0, 0, 0, 0, 0, 0};
int i;

/**
 * Divides a given PWM pin frequency by a divisor.
 *
 * The resulting frequency is equal to the base frequency divided by
 * the given divisor:
 *   - Base frequencies:
 *      o The base frequency for pins 3, 9, 10, and 11 is 31250 Hz.
 *      o The base frequency for pins 5 and 6 is 62500 Hz.
 *   - Divisors:
 *      o The divisors available on pins 5, 6, 9 and 10 are: 1, 8, 64,
 *        256, and 1024.
 *      o The divisors available on pins 3 and 11 are: 1, 8, 32, 64,
 *        128, 256, and 1024.
 *
 * PWM frequencies are tied together in pairs of pins. If one in a
 * pair is changed, the other is also changed to match:
 *   - Pins 5 and 6 are paired on timer0
 *   - Pins 9 and 10 are paired on timer1
 *   - Pins 3 and 11 are paired on timer2
 *
 * Note that this function will have side effects on anything else
 * that uses timers:
 *   - Changes on pins 3, 5, 6, or 11 may cause the delay() and
 *     millis() functions to stop working. Other timing-related
 *     functions may also be affected.
 *   - Changes on pins 9 or 10 will cause the Servo library to function
 *     incorrectly.
 *
 * Thanks to macegr of the Arduino forums for his documentation of the
 * PWM frequency divisors. His post can be viewed at:
 *   https://forum.arduino.cc/index.php?topic=16612#msg121031
 */
void setPwmFrequency(int pin, int divisor) {
  byte mode;
  if(pin == 5 || pin == 6 || pin == 9 || pin == 10) {
    switch(divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 64: mode = 0x03; break;
      case 256: mode = 0x04; break;
      case 1024: mode = 0x05; break;
      default: return;
    }
    if(pin == 5 || pin == 6) {
      TCCR0B = ((TCCR0B & 0b11111000) | mode);
    } else {
      TCCR1B = ((TCCR1B & 0b11111000) | mode);
    }
  } else if(pin == 3 || pin == 11) {
    switch(divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 32: mode = 0x03; break;
      case 64: mode = 0x04; break;
      case 128: mode = 0x05; break;
      case 256: mode = 0x06; break;
      case 1024: mode = 0x07; break;
      default: return;
    }
    TCCR2B = ((TCCR2B & 0b11111000) | mode);
  }
}

void receiveDataPacket(int howMany){
  if(fallback == true || watchDog == 0)
  {
    fallback = false;
    digitalWrite(LED_BUILTIN, LOW);
  }
  watchDog = 10;

  // get bytes from i2c
  bytesSent = 0; // clear byte counter
  for(byte i=0; i < howMany; i++){
    receivedPacket[i] = Wire.read();
  }

}

void slavesRespond(){
  if(bytesSent == 0){
    switch(receivedPacket[0]){
    case 1:  // digitalWrite
      /*
      receivedPacket[1] = pin
      receivedPacket[2] = value
      */
      pinMode(receivedPacket[1], OUTPUT); // set pin mode
      digitalWrite(receivedPacket[1], receivedPacket[2]);
      returninfo = 1;
      break;
    case 2:  // digitalRead
      /*
      receivedPacket[1] = pin
      */
      pinMode(receivedPacket[1], INPUT); // set pin mode
      returninfo = digitalRead(receivedPacket[1]);
      break;
    case 3:  // digitalRead pullup
      /*
      receivedPacket[1] = pin
      */
      pinMode(receivedPacket[1], INPUT_PULLUP); // set pin mode
      returninfo = digitalRead(receivedPacket[1]);
      break;
    case 4:  // analogWrite
      /*
      receivedPacket[1] = pin
      receivedPacket[2] = value
      */
      pinMode(receivedPacket[1], OUTPUT); // set pin mode
      analogWrite(receivedPacket[1], receivedPacket[2]);
      returninfo = 1;
      break;
    case 5:   // analogRead
      /*
      receivedPacket[1] = pin
      */
      returninfo = analogRead(receivedPacket[1]);
      break;
    case 10:  // analogWriteSlow
      /*
      receivedPacket[1] = pin
      receivedPacket[2] = value
      */
      if(pins[receivedPacket[1]] > 0) {
        pinMode(pins[receivedPacket[1]], OUTPUT); // set pin mode
        target[pins[receivedPacket[1]]] = receivedPacket[2];
        returninfo = 1;
      } else {
        returninfo = 0; 
      }
      break;
    }
  }
  if(bytesSent == 0){ //send first byte
    buffer[0] = returninfo >> 8;
    buffer[1] = returninfo & 0xff;
    Wire.write(buffer[0]);
    bytesSent++;
  } else if(bytesSent == 1){ // send second byte
    Wire.write(buffer[1]);
    bytesSent = 0; // clear byte counter
  }
}

void setup() {
  setPwmFrequency(3, 8);
  setPwmFrequency(5, 8);
  setPwmFrequency(6, 8);
  setPwmFrequency(9, 8);
  setPwmFrequency(10, 8);
  setPwmFrequency(11, 8);

  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);

  analogWrite(3, 0);
  analogWrite(5, 0);
  analogWrite(6, 0);
  analogWrite(9, 0);
  analogWrite(10, 0);
  analogWrite(11, 0);
  
  Wire.begin(SlaveDeviceId);         // join i2c bus with Slave ID
  Wire.onReceive(receiveDataPacket); // register talk event
  Wire.onRequest(slavesRespond);     // register callback event

  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
}

void adjust_pwm(int &l, int p, int m) {  
  if (l < m) {
    if (l >= m - 5) {
      l = m;
    } else {
      l += 5;
    }
    analogWrite(p, l);
  }

  if (l > m) {
    if (l <= 5) {
      l = 0;
    } else {
      l -= 5;
    }
    analogWrite(p, l);
  }
}

void loop() {
  for(i=1; i<8; i++) {
    if(target[i]!=current[i] || fallback && 255!=current[i]) {
      adjust_pwm(current[i],pin_map[i],fallback ? 255 : target[i]);
    }
  }
  
  if(millis() - tick >= 5000) {
    tick = millis();
    if(watchDog > 0)
    {
      watchDog--;
    }
    else
    {
      fallback = true;
      digitalWrite(LED_BUILTIN, HIGH);
    }    
  }

  delay(10);
}