Units.cpp

/*
 * Copyright (C) 2022 CERN
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 *
 * Created on: 2022-01-31
 *     Author: Michal Mysior <mmysior> <michal.mysior@cern.ch>
 *
 */
 
#include "Units.hpp"
 
#include <unordered_map>
 
#include <ccut/utils.hpp>
#include <math.h>
 
#include "Exception.hpp"
#include "util/serialize.hpp"
 
using smc::units::Unit;
 
namespace smc {
 
static const std::string s_empty;
static const std::unordered_map<Unit, std::string> s_unitToStr{
    {{units::UNKNOWN, "unknown"},
     {units::MILLIMETERS, "mm"},
     {units::METERS, "m"},
     {units::STEPS, "steps"},
     {units::DEGREES, "deg"},
     {units::RADIANS, "rad"},
     {units::TURNS, "turns"},
     {units::MILLIMETERS_PER_MINUTE, "mm/min"},
     {units::METERS_PER_SECOND, "m/s"},
     {units::STEPS_PER_MILLIMETER, "steps/mm"},
     {units::STEPS_PER_TURN, "steps/turn"},
     {units::MILLIMETERS_PER_SECOND, "mm/s"},
     {units::MILLIMETERS_PER_SECOND_SQUARED, "mm/s^2"}}};
static const std::unordered_map<std::string, Unit> s_unitFromStr{
    ccut::flip(s_unitToStr)};
static const std::set<Unit> s_unitKeys{ccut::keys(s_unitToStr)};
 
const std::string &to_string(Unit unit)
{
    std::unordered_map<Unit, std::string>::const_iterator it = s_unitToStr.find(
        unit);
    return (it != s_unitToStr.end()) ? it->second : s_empty;
}
 
template<>
Unit from_string<Unit>(const std::string &str)
{
    std::unordered_map<std::string, Unit>::const_iterator it =
        s_unitFromStr.find(str);
    return (it != s_unitFromStr.end()) ? it->second : Unit::UNKNOWN;
}
 
template<>
const std::set<Unit> &getEnumValues()
{
    return s_unitKeys;
}
 
namespace units {
 
Value::Value() : unit{UNKNOWN}, value{0.0} {}
 
Value::Value(Unit unit, value_type value) : unit{unit}, value{value} {}
 
bool Value::canConvert(unit_t unit) const
{
    value_type throwaway;
    return checkConvert(throwaway, unit);
}
 
bool Value::convert(unit_t unit)
{
    if (checkConvert(this->value, unit))
    {
        this->unit = unit;
        return true;
    }
    return false;
}
 
Value &Value::operator+=(value_type raw)
{
    this->value += raw;
    return *this;
}
 
Value &Value::operator-=(value_type raw)
{
    this->value -= raw;
    return *this;
}
 
Value &Value::operator*=(value_type raw)
{
    this->value *= raw;
    return *this;
}
 
Value &Value::operator/=(value_type raw)
{
    this->value /= raw;
    return *this;
}
 
Value operator*(Value::value_type raw, Unit unit)
{
    return Value{unit, raw};
}
 
Value operator"" _mm(long double raw)
{
    return Value{Unit::MILLIMETERS, Value::value_type(raw)};
}
 
Value operator"" _m(long double raw)
{
    return Value{Unit::METERS, Value::value_type(raw)};
}
 
Value operator"" _steps(long double raw)
{
    return Value{Unit::STEPS, Value::value_type(raw)};
}
 
Value operator"" _deg(long double raw)
{
    return Value{Unit::DEGREES, Value::value_type(raw)};
}
 
Value operator"" _rad(long double raw)
{
    return Value{Unit::RADIANS, Value::value_type(raw)};
}
 
Value operator"" _turns(long double raw)
{
    return Value{Unit::TURNS, Value::value_type(raw)};
}
 
Value operator"" _mm_per_min(long double raw)
{
    return Value{Unit::MILLIMETERS_PER_MINUTE, Value::value_type(raw)};
}
 
Value operator"" _m_per_s(long double raw)
{
    return Value{Unit::METERS_PER_SECOND, Value::value_type(raw)};
}
 
Value operator"" _steps_per_mm(long double raw)
{
    return Value{Unit::STEPS_PER_MILLIMETER, Value::value_type(raw)};
}
 
Value operator"" _steps_per_turn(long double raw)
{
    return Value{Unit::STEPS_PER_TURN, Value::value_type(raw)};
}
 
Value operator"" _mm_per_s(long double raw)
{
    return Value{Unit::MILLIMETERS_PER_SECOND, Value::value_type(raw)};
}
 
Value operator"" _mm_per_s2(long double raw)
{
    return Value{Unit::MILLIMETERS_PER_SECOND_SQUARED, Value::value_type(raw)};
}
 
Value operator*(const Value &value1, const Value &value2)
{
    switch (value1.unit)
    {
    case Unit::STEPS_PER_MILLIMETER:
        if (value2.unit == Unit::MILLIMETERS)
            return Value{Unit::STEPS, (value1.value * value2.value)};
        break;
    case Unit::MILLIMETERS:
        if (value2.unit == Unit::STEPS_PER_MILLIMETER)
            return Value{Unit::STEPS, (value1.value * value2.value)};
        break;
    case Unit::STEPS_PER_TURN:
        if (value2.unit == Unit::TURNS)
            return Value{Unit::STEPS, (value1.value * value2.value)};
        break;
    default: break;
    }
    throw Exception{ErrorCode::IncompatibleUnits};
}
 
Value operator/(const Value &value1, const Value &value2)
{
    switch (value1.unit)
    {
    case Unit::STEPS:
        if (value2.unit == Unit::MILLIMETERS)
            return Value{Unit::STEPS_PER_MILLIMETER,
                         (value1.value / value2.value)};
        else if (value2.unit == Unit::STEPS_PER_MILLIMETER)
            return Value{Unit::MILLIMETERS, (value1.value / value2.value)};
        else if (value2.unit == Unit::STEPS_PER_TURN)
            return Value{Unit::TURNS, (value1.value / value2.value)};
        else if (value2.unit == Unit::TURNS)
            return Value{Unit::STEPS_PER_TURN, (value1.value / value2.value)};
        break;
    default: break;
    }
    throw Exception{ErrorCode::IncompatibleUnits};
}
 
std::string Value::toString() const
{
    return (std::to_string(this->value) + to_string(this->unit));
}
 
bool Value::checkConvert(value_type &converted, Unit unit) const
{
    if (this->unit == unit)
    {
        return true;
    }
 
    bool ret = true;
    switch (this->unit)
    {
    // degrees <-> radians
    case unit_t::DEGREES:
        if (unit == unit_t::RADIANS)
            converted = (((this->value) * M_PI) / 180);
        else if (unit == unit_t::TURNS)
            converted = (this->value / 360);
        else
            ret = false;
        return ret;
    case unit_t::RADIANS:
        if (unit == unit_t::DEGREES)
            converted = (((this->value) * 180) / M_PI);
        else if (unit == unit_t::TURNS)
            converted = (this->value / (2 * M_PI));
        else
            ret = false;
        return ret;
    case unit_t::TURNS:
        if (unit == unit_t::DEGREES)
            converted = this->value * 360;
        else if (unit == unit_t::RADIANS)
            converted = this->value * (2 * M_PI);
        else
            ret = false;
        return ret;
    // mm/min -> m/s ; mm/min -> mm/s
    case Unit::MILLIMETERS_PER_MINUTE:
        if (unit == Unit::METERS_PER_SECOND)
        {
            converted = ((this->value / (60 * 1000)));
            return true;
        }
        else if (unit == Unit::MILLIMETERS_PER_SECOND)
        {
            converted = ((this->value / 60));
            return true;
        }
        return false;
 
    // m/s -> mm/min ; m/s -> mm/s
    case Unit::METERS_PER_SECOND:
        if (unit == Unit::MILLIMETERS_PER_MINUTE)
        {
            converted = ((this->value * (60 * 1000)));
            return true;
        }
        else if (unit == Unit::MILLIMETERS_PER_SECOND)
        {
            converted = ((this->value * 1000));
            return true;
        }
        return false;
 
    // mm/s -> m/s ; mm/s -> mm/min
    case Unit::MILLIMETERS_PER_SECOND:
        if (unit == Unit::METERS_PER_SECOND)
        {
            converted = ((this->value / 1000));
            return true;
        }
        else if (unit == Unit::MILLIMETERS_PER_MINUTE)
        {
            converted = ((this->value * 60));
            return true;
        }
        return false;
 
    // mm -> m
    case Unit::MILLIMETERS:
        if (unit == Unit::METERS)
        {
            converted = ((this->value / 1000));
            return true;
        }
        return false;
 
    // m -> mm
    case Unit::METERS:
        if (unit == Unit::MILLIMETERS)
        {
            converted = ((this->value * 1000));
            return true;
        }
        return false;
 
    // default
    default: return false;
    }
}
 
std::string IOPort::toString(IOPort::StrFmt format) const
{
    if (size == 0)
        return std::string();
    else if (format == StrFmt::AUTO && size == 1)
        return value ? "1" : "0";
    else if ((format == StrFmt::BIN) ||
             ((format == StrFmt::AUTO) && (size <= 16)))
    {
        std::vector<char> buf(size + (size >> 3) + 3);
        size_t off = buf.size() - 1;
        uint64_t v = value;
 
        buf[off--] = '\0';
        /* binary notation */
        for (size_t i = 0; i < size; ++i)
        {
            if (i && !(i & 0x07))
                buf[off--] = '_';
            buf[off--] = (v & 1) ? '1' : '0';
            v >>= 1;
        }
        buf[off--] = 'b';
        buf[off] = '0';
        return &buf[off];
    }
    else
    {
        std::vector<char> buf(size + (size >> 5) + 3);
        size_t off = buf.size() - 1;
        uint64_t v = value;
 
        buf[off--] = '\0';
        /* hex notation */
        for (size_t i = 0; i < size;)
        {
            if (i && !(i & 0x1F))
                buf[off--] = '_';
            const uint8_t quartet = (v & 0x0F);
            buf[off--] = (quartet >= 10) ? (quartet - 10 + 'A') :
                                           (quartet + '0');
            v >>= 4;
            i += 4;
        }
        buf[off--] = 'x';
        buf[off] = '0';
        return &buf[off];
    }
}
 
} /* namespace units */
 
template<>
units::io_port_t from_string<units::io_port_t>(const std::string &str)
{
    if (str.empty())
        return units::IOPort{};
    else if (str.size() == 1)
    {
        switch (str[0])
        {
        case '0': return units::IOPort{false};
        case '1': return units::IOPort{true};
        default: return units::IOPort{};
        }
    }
    else if (str[0] == '0' && (str[1] == 'x' || str[1] == 'X'))
    {
        uint64_t value = 0;
        size_t sz = 0;
        for (size_t i = 2; i < str.size(); ++i)
        {
            const char c = str[i];
            if (c == '_')
                continue;
 
            value <<= 4;
            sz += 4;
            if (c >= '0' && c <= '9')
                value += c - '0';
            else if (c >= 'a' && c <= 'f')
                value += c - 'a' + 10;
            else if (c >= 'A' && c <= 'F')
                value += c - 'A' + 10;
            else
                return units::IOPort{};
        }
        return units::IOPort{value, sz};
    }
    else if (str[0] == '0' && (str[1] == 'b' || str[1] == 'B'))
    {
        uint64_t value = 0;
        size_t sz = 0;
        for (size_t i = 2; i < str.size(); ++i)
        {
            const char c = str[i];
            if (c == '_')
                continue;
 
            value <<= 1;
            ++sz;
            if (c == '1')
                value |= 1;
            else if (c != '0')
                return units::IOPort{};
        }
        return units::IOPort{value, sz};
    }
    else
        return units::IOPort{};
}
 
} /* namespace smc */