Arduino extended timer1

timer1.h

#include <stdint.h>
#include <Arduino.h>
#ifndef _AVR_IOXXX_H_
#include <avr/iom328.h> // Arduino Uno uses AtMega328p microchip
#endif

uint32_t us2cts(uint32_t us)
{
    return us >> 2;
}

typedef struct
{
    int16_t cycle;
    int16_t n_cycles;
} ISRcounter;

volatile ISRcounter t1ovflow = {0, 0};
volatile ISRcounter t1matchA = {0, 0};
volatile ISRcounter t1matchB = {0, 0};

class Timer1
{
public:
    Timer1(uint32_t ICR1_us, uint32_t OCR1A_us, uint32_t OCR1B_us);
    uint32_t ICR1_32;
    uint32_t OCR1A_32;
    uint32_t OCR1B_32;
    void set_ICR1(uint32_t us);
    void set_OCR1A(uint32_t us);
    void set_OCR1B(uint32_t us);
    void rewind_time(int32_t us);
    uint32_t get_time();
    void pause();
    void run();

    void (*overfl_handler)() = []()
    { ; };
    void (*matchA_handler)() = []()
    { ; };
    void (*matchB_handler)() = []()
    { ; };
};

Timer1::Timer1(uint32_t ICR1_us, uint32_t OCR1A_us, uint32_t OCR1B_us)
{
    set_ICR1(ICR1_us);
    set_OCR1A(OCR1A_us);
    set_OCR1B(OCR1B_us);
}

void Timer1::set_ICR1(uint32_t us)
{
    ICR1_32 = us;
    uint32_t cts = us2cts(us);

    t1ovflow.cycle = 0;
    t1ovflow.n_cycles = 1;
    // We divide required #counts in half until if fits into 16bit
    while (cts >= UINT16_MAX)
    {
        cts >>= 1;               // divide in half
        t1ovflow.n_cycles <<= 1; // double number of cycles
    }

    ICR1 = cts - 1;
}

void Timer1::set_OCR1A(uint32_t us)
{
    OCR1A_32 = us;
    uint32_t cts = us2cts(us);

    t1matchA.cycle = 0;
    t1matchA.n_cycles = cts / (ICR1 + 1);

    OCR1A = cts % ICR1;
}

void Timer1::set_OCR1B(uint32_t us)
{
    OCR1B_32 = us;
    uint32_t cts = us2cts(us);

    t1matchB.cycle = 0;
    t1matchB.n_cycles = cts / (ICR1 + 1);

    OCR1B = cts % ICR1;
}

void Timer1::rewind_time(int32_t us)
{
    int32_t cts;
    if (us >= 0)
    {
        cts = us2cts(us);
    }
    else
    {
        cts = -us2cts(-us);
    }

    uint16_t N = ICR1 + 1;
    t1ovflow.cycle += cts / N;

    uint16_t adj = abs(cts) % N;

    TCNT1 = (cts >= 0) ? TCNT1 + adj : TCNT1 - adj;
}

void Timer1::get_time()
{
    return ICR1 * t1ovflow.cycle + TCNT1;
}

void Timer1::pause()
{
    GTCCR = bit(TSM) | bit(PSRSYNC);
}

void Timer1::run()
{
    GTCCR = 0;
}

Timer1 timer1 = Timer1();

// --------------------------------------------------------------

ISR(TIMER1_COMPA_vect) // interrupt 11
{
    // It's time to call interrupt handler
    if (t1matchA.cycle++ == t1matchA.n_cycles)
    {
        timer1.matchA_handler();
    }
}

ISR(TIMER1_COMPB_vect) // interrupt 12
{
    // It's time to call interrupt handler
    if (t1matchB.cycle++ == t1matchB.n_cycles)
    {
        timer1.matchB_handler();
    }
}

ISR(TIMER1_OVF_vect) // interrupt 13
{
    // It's time to call interrupt handler
    if (++t1ovflow.cycle >= t1ovflow.n_cycles)
    {
        timer1.overfl_handler();
        t1ovflow.cycle = 0;
        t1matchA.cycle = 0;
        t1matchB.cycle = 0;
    }
}

void one()
{
    PINC |= bit(PINC0);
}
void two()
{
    PINC |= bit(PINC1);
}
void three()
{
    PINC |= bit(PINC2);
}

// the setup function runs once when you press reset or power the board
void setup()
{
    // initialize digital pin LED_BUILTIN as an output.
    DDRC = 0xFF;
    // WGM mode 14 (fast PWM with ICR1 max)
    TCCR1A = bit(WGM11);
    TCCR1B = bit(WGM12) | bit(WGM13) | bit(CS10) | bit(CS11);

    // Set timing registers
    ICR1 = UINT16_MAX;

    // Enable interrupts for overflow, match A, and match B
    TIMSK1 = bit(TOIE1) | bit(OCIE1A) | bit(OCIE1B);

    Serial.begin(2000000);
    Serial.setTimeout(10); // ms

    // Wait until the serial port is ready
    while (!Serial)
    {
    }
    Serial.flush();

    // Notify the host that we are ready
    Serial.print("Arduino is ready. Firmware version: ");
    Serial.print("0.3.0\n");

    timer1.set_ICR1(1000);
    timer1.set_OCR1A(300);
    timer1.set_OCR1B(400);
}

#pragma pack(push) /* push current alignment to stack */
#pragma pack(1)    /* set alignment to 1 byte boundary */

// Address and value of register for read/write operations
typedef struct
{
    uint8_t addr;
    uint8_t value;
} Register;

// Data packet for serial communication
union Data
{
    struct
    {
        uint8_t cmd;

        // All members below share the same chunk of memory
        union
        {
            Register R;   // register access (R/W)
            int32_t TCNT; // fluidics injection delay. If negative, happens before imaging
            uint32_t N;   // time between frames in any imaging mode
        };
    };

    uint8_t bytes[9];
} data;

#define MEM_IO_8bit(mem_addr) (*(volatile uint8_t *)(uintptr_t)(mem_addr))

void parse_UART_command()
{
    switch (data.cmd)
    {
    // Read register
    case 'R':
        Serial.write(MEM_IO_8bit(data.R.addr));
        break;

    // Write register
    case 'W':
        MEM_IO_8bit(data.R.addr) = data.R.value;
        break;

    case 'I':
        timer1.set_ICR1(data.N);
        break;

    case 'A':
        timer1.set_OCR1A(data.N);
        break;

    case 'B':
        timer1.set_OCR1B(data.N);
        break;

    case 'T':
        timer1.rewind_time(data.TCNT);
        break;

    case 'P':
        timer1.pause();
        break;

    case 'r':
        timer1.overfl_handler = one;
        timer1.matchA_handler = two;
        timer1.matchB_handler = three;
        timer1.run();
        break;

    default:
        break;
    }
}

// the loop function runs over and over again forever
void loop()
{
    if (Serial.available())
    {
        if (Serial.readBytes(data.bytes, 5) == 5)
        {
            parse_UART_command();
        }
    }
}