#include <stdint.h>

#ifndef REGISTERS_H
#define REGISTERS_H

/**
 * @brief Writes data on the given position in register.
 * @param[in,out] reg The address to change.
 * @param[in] data The data to write on pos.
 * @param[in] pos The position to start writing to. First @see data param is shifted by this.
 * @param[in] mask The mask to and the data with, ie. if you want only two bits, set to 0x3.
 */
inline void __attribute__((always_inline))
reg_write_bits_pos(volatile uint32_t *reg, uint32_t data, uint8_t pos,
                   uint32_t mask) {
  uint32_t val = *reg;
  val &= ~(mask << pos);
  val |= (data & mask) << pos;
  *reg = val;
}

/**
 * @brief Writes data into the register.
 * @param[in,out] reg The address to change.
 * @param[in] data The data to write to the whole register.
 * @param[in] mask The mask to and the data with, ie. if you want only two bits, set to 0x3.
 */
inline void __attribute__((always_inline)) reg_write_bits(volatile uint32_t *reg, uint32_t data,
                                  uint32_t mask) {
  uint32_t val = *reg;
  val &= ~(mask);
  val |= (data & mask);
  *reg = val;
}

/**
 * @brief Write ones on the given position in register.
 * @param[in,out] reg The address to change.
 * @param[in] pos The position to start writing to. First @see mask param is shifted by this.
 * @param[in] mask The mask of bits to write ones to, ie. if you want only two bits, set to 0x3.
 */
inline void __attribute__((always_inline)) reg_set_bits_pos(volatile uint32_t *reg, uint8_t pos,
                                    uint32_t mask) {
  *reg |= mask << pos;
}

/**
 * @brief Write ones into register.
 * @param[in,out] reg The address to change.
 * @param[in] mask The mask of bits to write ones to.
 */
inline void __attribute__((always_inline)) reg_set_bits(volatile uint32_t *reg,
                                                        uint32_t mask) {
  *reg |= mask;
}

/**
 * @brief Toggle bits in the register, starting from @see pos.
 * @details Toggles bits where one is written to the mask, but shifted by @see pos.
 * @param[in,out] reg Address of the register to toggle bits in.
 * @param[in] pos The position to shift the @see mask by.
 * @param[in] mask The mask saying what bits to toggle (1), from the @see pos.
 */
inline void __attribute__((always_inline)) reg_toggle_bits_pos(volatile uint32_t *reg, uint8_t pos,
                                       uint32_t mask) {
  *reg ^= (mask << pos);
}

/**
 * @brief Toggle bits in the register.
 * @details Toggles bits where one is written to the mask.
 * @param[in,out] reg Address of the register to toggle bits in.
 * @param[in] mask The mask saying what bits to toggle (1).
 */
inline void __attribute__((always_inline))
reg_toggle_bits(volatile uint32_t *reg, uint32_t mask) {
  *reg ^= mask;
}

/**
 * @brief Writes zeros on the given position in register.
 * @param[in,out] reg The address to change.
 * @param[in] pos The position to start writing to. First the mask is shifted by this.
 * @param[in] mask The mask of bits to write ones to, ie. if you want only two bits, set to 0x3.
 */
inline void __attribute__((always_inline))
reg_clear_bits_pos(volatile uint32_t *reg, uint8_t pos, uint32_t mask) {
  *reg ^= ~(mask << pos);
}

/**
 * @brief Write zeros on into register.
 * @param[in,out] reg The address to change.
 * @param[in] mask The mask of bits to write ones to.
 */
inline void __attribute__((always_inline))
reg_clear_bits(volatile uint32_t *reg, uint32_t mask) {
  *reg &= ~mask;
}

/**
 * @brief Get bits from the register on the given position.
 * @details The obtained value starts from @see pos, and is anded with @see mask.
 * @param[in,out] reg The register address to read
 * @param[in] pos The position to shift data by.
 * @param[in] mask The mask to apply on the resulting data
 * @return The read data.
 */
inline uint32_t __attribute__((always_inline))
reg_read_bits_pos(volatile uint32_t *reg, uint8_t pos, uint32_t mask) {
  return ((*reg) >> pos) & mask;
}

#endif // REGISTERS_H