#include <Wire.h>
#include "pwm.h"


const uint8_t I2C_ADDRESS = 0x09;
const uint8_t MAX_PINS = 18;
const uint32_t PWM_PERIOD = 20000L; // 50Hz

volatile uint8_t *ports[MAX_PINS] = {
 &PORTD, &PORTD,
 &PORTD, &PORTD, &PORTD, &PORTD, &PORTD, &PORTD, &PORTB, &PORTB,
 &PORTB, &PORTB, &PORTB, &PORTB, &PORTC, &PORTC, &PORTC, &PORTC,
};
uint8_t pins[MAX_PINS] = {
    1, 0,
    2, 3, 4, 5, 6, 7, 0, 1,
    2, 3, 4, 5, 0, 1, 2, 3,
};
volatile bool changed = false;


void receive(int16_t count) {
    union {
        byte bytes[2];
        uint16_t value;
    } bytes2int;
    uint8_t servo = Wire.read();
    while ((Wire.available() >= 2) && (servo < MAX_PINS)) {
        bytes2int.bytes[0] = Wire.read();
        bytes2int.bytes[1] = Wire.read();
        pwm_set_phase_batch(servo, bytes2int.value);
        ++servo;
    }
    while (Wire.available()) {
        Wire.read();
    }
    changed = true;
}


void setup() {
    pwm_init(ports, pins, MAX_PINS, PWM_PERIOD);
    for (uint8_t i = 0; i < MAX_PINS; ++i) {
        pwm_set_phase_batch(i, 0);
    }
    pwm_apply_batch();
    Wire.begin(I2C_ADDRESS);
    Wire.onReceive(receive);
}


void loop() {
    if (changed) {
        pwm_apply_batch();
        changed = false;
    }
}