#include "mgmLis3Helpers.h"

uint8_t mgmLis3::readCommand(uint8_t command, bool continuousCom) {
  command |= (1 << mgmLis3::RW_BIT);
  if (continuousCom == true) {
    command |= (1 << mgmLis3::MS_BIT);
  }
  return command;
}

uint8_t mgmLis3::writeCommand(uint8_t command, bool continuousCom) {
  command &= ~(1 << mgmLis3::RW_BIT);
  if (continuousCom == true) {
    command |= (1 << mgmLis3::MS_BIT);
  }
  return command;
}

mgmLis3::Sensitivies mgmLis3::getSensitivity(uint8_t ctrlRegister2) {
  bool fs0Set = ctrlRegister2 & (1 << mgmLis3::FSO);  // Checks if FS0 bit is set
  bool fs1Set = ctrlRegister2 & (1 << mgmLis3::FS1);  // Checks if FS1 bit is set

  if (fs0Set && fs1Set)
    return mgmLis3::Sensitivies::GAUSS_16;
  else if (!fs0Set && fs1Set)
    return mgmLis3::Sensitivies::GAUSS_12;
  else if (fs0Set && !fs1Set)
    return mgmLis3::Sensitivies::GAUSS_8;
  else
    return mgmLis3::Sensitivies::GAUSS_4;
}

float mgmLis3::getSensitivityFactor(mgmLis3::Sensitivies sens) {
  switch (sens) {
    case (mgmLis3::GAUSS_4): {
      return mgmLis3::FIELD_LSB_PER_GAUSS_4_SENS;
    }
    case (mgmLis3::GAUSS_8): {
      return mgmLis3::FIELD_LSB_PER_GAUSS_8_SENS;
    }
    case (mgmLis3::GAUSS_12): {
      return mgmLis3::FIELD_LSB_PER_GAUSS_12_SENS;
    }
    case (mgmLis3::GAUSS_16): {
      return mgmLis3::FIELD_LSB_PER_GAUSS_16_SENS;
    }
    default: {
      // Should never happen
      return mgmLis3::FIELD_LSB_PER_GAUSS_4_SENS;
    }
  }
}