toys/dbcc/source/signal.cpp

#include "dbcc/signal.h"

#include <algorithm>
#include <iterator>
#include <limits>
#include <sstream>
#include <vector> /**< std::vector */

#include "dbcc/helper/signal_helper.h"

// FIXME: We ignore the properties and comment of Signal
namespace ad {
namespace dbcc {

static inline std::string& Trim(std::string& str, const std::string& toTrim = " ")
{
    std::string::size_type pos = str.find_last_not_of(toTrim);

    if (pos != std::string::npos) {
        str.erase(pos + 1);
        str.erase(0, str.find_first_not_of(toTrim));
    }

    return str;
}

static inline std::vector<std::string>& Split(const std::string& s, char delim,
                                              std::vector<std::string>& elems)
{
    std::stringstream ss(s);
    std::string item;

    while (std::getline(ss, item, delim)) {
        elems.push_back(Trim(item));
    }

    return elems;
}

static inline std::vector<std::string> Split(const std::string& s, char delim)
{
    std::vector<std::string> elems;
    Split(s, delim, elems);
    return elems;
}

static inline bool ConsumeNumber(std::istream& in, double& num)
{
    int64_t i = 0;
    auto pos = in.tellg();

    in >> i;

    char c = in.peek();

    if (c == '.') {
        // Revert to the previous position
        in.seekg(pos);
        // Convert as a double
        double d = 0;
        in >> d;
        num = d;
        return true;
    }

    num = i;

    return false;
}

std::istream& operator>>(std::istream& in, Signal& sig)
{
    sig.is_float_ = false;
    // Parse the signal name
    in >> sig.name_;

    std::string multi;
    in >> multi;

    // This case happens if there is not Multiplexor present
    if (multi == ":") {
        sig.multiplexor_ = Multiplexor::kNone;
    } else {
        if (multi == "M") {
            sig.multiplexor_ = Multiplexor::kMultiplexor;
        } else {
            // The multiplexor looks like that 'm12' so we ignore the m and parse it as integer
            std::istringstream multistream(multi);
            multistream.ignore(1);

            uint16_t multi_num;
            multistream >> multi_num;
            sig.multiplexor_ = Multiplexor::kMultiplexor;
            sig.multiplexed_number_ = multi_num;
        }

        // Ignore the next character which is a ':'
        in >> multi;
    }

    in >> sig.start_bit_;
    in.ignore(1);
    in >> sig.length_;
    in.ignore(1);

    int order;
    in >> order;

    if (order == 0) {
        sig.byte_order_ = ByteOrder::kMotorola;
    } else {
        sig.byte_order_ = ByteOrder::kIntel;
    }

    char sign;
    in >> sign;

    if (sign == '+') {
        sig.sign_ = Sign::kUnsigned;
    } else {
        sig.sign_ = Sign::kSigned;
    }

    bool is_float = false;
    // (factor,offset) [max|min]
    in.ignore(std::numeric_limits<std::streamsize>::max(), '(');
    is_float = ConsumeNumber(in, sig.factor_) || is_float;
    in.ignore(1); // ,
    is_float = ConsumeNumber(in, sig.offset_) || is_float;
    in.ignore(1); // )
    in.ignore(std::numeric_limits<std::streamsize>::max(), '[');
    is_float = ConsumeNumber(in, sig.minimum_) || is_float;
    in.ignore(1); // |
    is_float = ConsumeNumber(in, sig.maximum_) || is_float;
    in.ignore(1); // ]

    sig.is_float_ = is_float;

    // Unit string
    std::stringstream unit;
    in.ignore(std::numeric_limits<std::streamsize>::max(), '\"');

    while (in.peek() != '\"') {
        unit.put(in.get());
    }

    if (in.eof() || !in) {
        in.setstate(std::ios_base::failbit);
        return in;
    }

    in.ignore(1); // the tail "

    sig.unit_ = unit.str();
    sig.unit_ = Trim(sig.unit_, "\"");
    sig.unit_ = Trim(sig.unit_);

    std::string to;
    getline(in, to);

    if (!to.empty() && *to.rbegin() == '\r') {
        to.erase(to.length() - 1, 1);
    }

    if (!to.empty()) {
        std::vector<std::string> toStrings = Split(to, ',');
        std::move(toStrings.begin(), toStrings.end(), std::inserter(sig.to_, sig.to_.begin()));
    }

    return in;
}

Signal::~Signal()
{}

bool Signal::Compile()
{
    ad::dbcc::helper::SignalHelper sig_helper(*this);

    for (auto seg : sig_helper.Segments(true)) {
        CompiledSignalParameter param;

        if (seg.direction == ad::dbcc::helper::SignalHelper::ShiftDirection::kLeft) {
            param.left = 1;
        } else {
            param.left = 0;
        }

        param.index = seg.index;
        param.shift = seg.shift;
        param.mask = seg.mask;

        params_.emplace_back(param);
    }

    if (GetSign() == ad::dbcc::Sign::kSigned) {
        uint32_t mask = ((1 << (sig_helper.TypeLength() - Length())) - 1);

        if (mask != 0) {
            mask <<= Length();
            signed_length_mask_ = mask;
        }
    }

#if defined(DEBUG) && defined(VERBOSE)
    std::cout << *this << std::endl;
#endif /* DEBUG && VERBOSE */

    return true;
}

static uint8_t PackLeftShift(uint64_t value, uint32_t shift, uint32_t mask)
{
    return (uint8_t)((uint8_t)(value << shift) & mask);
}

static uint8_t PackRightShift(uint64_t value, uint32_t shift, uint32_t mask)
{
    return (uint8_t)((uint8_t)(value >> shift) & mask);
}

static int64_t UnpackLeftShift(uint8_t value, uint32_t shift, uint32_t mask)
{
    return static_cast<int64_t>(static_cast<int64_t>(value & mask) << shift);
}

static int64_t UnpackRightShift(uint8_t value, uint32_t shift, uint32_t mask)
{
    return static_cast<int64_t>(static_cast<int64_t>(value & mask) >> shift);
}

bool Signal::Encode(const ParsedValue& pv, uint8_t* data, size_t /* length */)
{
    if (params_.size() == 0) {
        Compile();
    }

    if (pv.is_integer == IsFloat()) {
        return false;
    }

    int64_t temp = 0;

    if (pv.is_integer) {
        temp = static_cast<int64_t>((pv.i - Offset()) / Factor());
    } else {
        temp = static_cast<int64_t>((pv.f - Offset()) / Factor());
    }

    for (auto& p : params_) {
        if (p.left) {
            data[p.index] |= PackRightShift(temp, p.shift, p.mask);
        } else {
            data[p.index] |= PackLeftShift(temp, p.shift, p.mask);
        }
    }

    return true;
}

bool Signal::Decode(const uint8_t* data, size_t /* length */, ParsedValue& pv)
{
    if (params_.size() == 0) {
        Compile();
    }

    int64_t result = 0;

    pv.is_integer = !IsFloat();

    for (auto& p : params_) {
        if (p.left) {
            result |= UnpackLeftShift(data[p.index], p.shift, p.mask);
        } else {
            result |= UnpackRightShift(data[p.index], p.shift, p.mask);
        }
    }

    if (signed_length_mask_ != 0) {
        if ((result & (static_cast<int64_t>(1) << (Length() - 1))) != 0) {
            result |= signed_length_mask_;
        }
    }

    if (pv.is_integer) {
        pv.i = static_cast<int32_t>(result * Factor() + Offset());
    } else {
        pv.f = static_cast<float>(result * Factor() + Offset());
    }

    return true;
}

bool Signal::Encode(double value, uint8_t* data, size_t /* length */)
{
    if (params_.size() == 0) {
        Compile();
    }

    int64_t temp = 0;

    if (!IsFloat()) {
        int32_t i = static_cast<int32_t>(value);
        temp = static_cast<int64_t>((i - Offset()) / Factor());
    } else {
        temp = static_cast<int64_t>((value - Offset()) / Factor());
    }

    for (auto& p : params_) {
        if (p.left) {
            data[p.index] |= PackRightShift(temp, p.shift, p.mask);
        } else {
            data[p.index] |= PackLeftShift(temp, p.shift, p.mask);
        }
    }

    return true;
}

bool Signal::Decode(const uint8_t* data, size_t /* length */, double& value)
{
    if (params_.size() == 0) {
        Compile();
    }

    int64_t result = 0;

    for (auto& p : params_) {
        if (p.left) {
            result |= UnpackLeftShift(data[p.index], p.shift, p.mask);
        } else {
            result |= UnpackRightShift(data[p.index], p.shift, p.mask);
        }
    }

    if (signed_length_mask_ != 0) {
        if ((result & (static_cast<int64_t>(1) << (Length() - 1))) != 0) {
            result |= signed_length_mask_;
        }
    }

    if (!IsFloat()) {
        value = static_cast<double>(static_cast<int32_t>(result * Factor() + Offset()));
    } else {
        value = static_cast<double>(result * Factor() + Offset());
    }

    return true;
}

// Here I assume the layout:
//
//                        Bit
//
//           7   6   5   4   3   2   1   0
//         +---+---+---+---+---+---+---+---+
//       0 |<-x|<---------------------x|<--|
//         +---+---+---+---+---+---+---+---+
//       1 |-------------------------------|
//         +---+---+---+---+---+---+---+---+
//       2 |----------x|   |   |   |   |   |
//   B     +---+---+---+---+---+---+---+---+
//   y   3 |   |   |   |   |   |   |   |   |
//   t     +---+---+---+---+---+---+---+---+
//   e   4 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//       5 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//       6 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//       7 |   |   |   |   |   |   |   |   |
//         +---+---+---+---+---+---+---+---+
//
// Reference: https://github.com/cantools/cantools#the-dump-subcommand
bool Signal::BitsLayout(LayoutInfo& info)
{
    info.bits.clear();
    info.byte_lines.clear();
    info.byte_line_range.clear();

    if (byte_order_ == ByteOrder::kIntel) //< little-endian
    {
        int line_start = start_bit_ / 8;
        int remainder = 8 - start_bit_ % 8;
        int line_num = ((length_ - remainder + 7) / 8) + 1;
        int start_bit = start_bit_;
        int end_bit = start_bit;
        if (remainder > length_) {
            end_bit += length_;
        } else {
            end_bit += remainder;
        }

        remainder = length_;

        for (int line = line_start; line < line_start + line_num; ++line) {
            for (int bit_idx = start_bit; bit_idx < end_bit; bit_idx++) {
                info.bits.push_back(bit_idx);
            }

            info.byte_lines.push_back(line);
            info.byte_line_range.emplace_back(
                std::pair<int, int>{(9 - start_bit % 8) - 1, (8 - (end_bit - 1) % 8) - 1});

            remainder = remainder - (start_bit - end_bit);
            start_bit = 8 * (line + 1);

            if (remainder >= 8) {
                end_bit = start_bit + 8;
            } else {
                end_bit = start_bit + remainder;
            }
        }
    } else { //< big-endian
        int line_start = start_bit_ / 8;
        int remainder = (8 - (8 - (start_bit_ + 1) % 8) % 8);
        int line_num = ((length_ - remainder + 7) / 8) + 1;
        int start_bit = start_bit_;
        int line_remainder = (start_bit % 8) + 1;
        int end_bit = start_bit;
        if (line_remainder > length_) {
            end_bit -= length_;
        } else {
            end_bit -= line_remainder;
        }

        remainder = length_;

        for (int line = line_start; line < line_start + line_num; ++line) {
            for (int bit_idx = start_bit; bit_idx > end_bit; bit_idx--) {
                info.bits.push_back(bit_idx);
            }

            info.byte_lines.push_back(line);
            info.byte_line_range.emplace_back(
                std::pair<int, int>{(9 - (end_bit + 1) % 8) - 1, (8 - start_bit % 8) - 1});
            remainder = remainder - (start_bit - end_bit);
            start_bit = 8 * (line + 2) - 1;
            if (remainder >= 8) {
                end_bit = start_bit - 8;
            } else {
                end_bit = start_bit - remainder;
            }
        }
    }

    return true;
}

std::ostream& operator<<(std::ostream& out, const Signal::CompiledSignalParameter& p)
{
    out << "CompiledSignalParameter: { index: " << p.index << ", "
        << "shift: " << p.shift << ", "
        << "mask: 0x" << std::hex << p.mask << std::dec << ", "
        << "dir: " << (p.left == 1 ? "Left" : "Right") << " }";

    return out;
}

std::ostream& operator<<(std::ostream& out, const Signal::LayoutInfo& p)
{
    out << "Bits: { ";
    for (auto bit : p.bits) {
        out << bit << ", ";
    }
    out << " }\n";

    for (size_t idx = 0; idx < p.byte_lines.size(); idx++) {
        out << "Line(" << p.byte_lines[idx] << "): [" << p.byte_line_range[idx].first << ", "
            << p.byte_line_range[idx].second << "]\n";
    }

    return out;
}

std::ostream& operator<<(std::ostream& out, const Signal& cs)
{
    int i = 0;
    out << "Message: " << cs.GetMessageId() << ", Signal: " << cs.Name()
        << (cs.IsFloat() ? ", float" : "") << std::endl;

    for (auto& p : cs.params_) {
        out << "  param" << i << ": " << p << std::endl;
        i++;
    }

    if (cs.signed_length_mask_ != 0) {
        out << "  mask: 0x" << std::hex << cs.signed_length_mask_ << std::dec << std::endl;
    }

    out << "  factor: " << cs.Factor() << ", offset: " << cs.Offset();

    return out;
}

std::string Signal::ToDbcString() const
{
    std::ostringstream oss;
    oss << "SG_ " << Name();

    if (GetMultiplexor() == Multiplexor::kMultiplexor) {
        oss << " M" << MultiplexedNumber() << ": ";
    } else if (GetMultiplexor() == Multiplexor::kNone) {
        oss << " : ";
    } else {
        oss << " M: ";
    }

    oss << StartBit() << "|" << Length() << "@"
        << (GetByteOrder() == ByteOrder::kMotorola ? 0 : 1)
        << (GetSign() == Sign::kUnsigned ? "+" : "-") << " ("
        << Factor() << "," << Offset() << ") ["
        << Minimum() << "|" << Maximum() << "] \""
        << Unit() << "\"";

    if (!to_.empty()) {
        char delimiter = ' ';
        for (auto t : to_) {
            oss << delimiter << t;
            delimiter = ',';
        }
    }

    return oss.str();
}

} // namespace dbcc
} // namespace ad