From 8a68d61ee5c509ad3ae08711b8bbd22f9971eaaa Mon Sep 17 00:00:00 2001
From: FerroO2000 <immersivetech.mod@gmail.com>
Date: Fri, 24 May 2024 10:33:14 +0200
Subject: [PATCH] CANIDBuilder improvements

---
 bus.go                |  3 +++
 canid_builder.go      | 32 +++++++++++++++++++++++++++++++-
 canid_builder_test.go | 17 +++++++++++++----
 utils.go              |  7 ++++---
 utils_test.go         |  3 ++-
 5 files changed, 53 insertions(+), 9 deletions(-)

diff --git a/bus.go b/bus.go
index b52becb..1f3cdc4 100644
--- a/bus.go
+++ b/bus.go
@@ -267,6 +267,9 @@ func (b *Bus) Baudrate() int {
 
 // SetCANIDBuilder sets the [CANIDBuilder] of the [Bus].
 func (b *Bus) SetCANIDBuilder(canIDBuilder *CANIDBuilder) {
+	if b.canIDBuilder != nil {
+		b.canIDBuilder.removeRef(b.entityID)
+	}
 	b.canIDBuilder = canIDBuilder
 }
 
diff --git a/canid_builder.go b/canid_builder.go
index efa1a7c..289c9fe 100644
--- a/canid_builder.go
+++ b/canid_builder.go
@@ -5,7 +5,7 @@ import (
 	"strings"
 )
 
-var defaulCANIDBuilder = NewCANIDBuilder("default CAN-ID builder").UseNodeID(0, 4).UseMessageID(4, 7)
+var defaulCANIDBuilder = NewCANIDBuilder("default CAN-ID builder").UseNodeID(0, 4).UseMessageID(4, 7).UseCAN2A()
 
 // CANID is the CAN-ID of a [Message] within a [Bus].
 // Every message should have a different CAN-ID.
@@ -25,6 +25,8 @@ const (
 	// CANIDBuilderOpKindNodeID represents an operation
 	// that involves the node id.
 	CANIDBuilderOpKindNodeID
+	// CANIDBuilderOpKindBitMask represents a bit masking operation.
+	CANIDBuilderOpKindBitMask
 )
 
 func (bok CANIDBuilderOpKind) String() string {
@@ -35,6 +37,8 @@ func (bok CANIDBuilderOpKind) String() string {
 		return "message-id"
 	case CANIDBuilderOpKindNodeID:
 		return "node-id"
+	case CANIDBuilderOpKindBitMask:
+		return "bit-mask"
 	default:
 		return "unknown"
 	}
@@ -115,6 +119,12 @@ func (b *CANIDBuilder) Calculate(messagePriority MessagePriority, messageID Mess
 	canID := uint32(0)
 
 	for _, op := range b.operations {
+		if op.kind == CANIDBuilderOpKindBitMask {
+			mask := uint32(0xFFFFFFFF) >> uint32(32-op.len)
+			canID &= (mask << uint32(op.from))
+			continue
+		}
+
 		tmpVal := uint32(0)
 		switch op.kind {
 		case CANIDBuilderOpKindMessagePriority:
@@ -165,3 +175,23 @@ func (b *CANIDBuilder) UseNodeID(from, len int) *CANIDBuilder {
 	})
 	return b
 }
+
+// UseCAN2A adds a bit mask from 0 with a length of 11,
+// which makes the calculated CAN-ID conformed to the CAN 2.0A.
+func (b *CANIDBuilder) UseCAN2A() *CANIDBuilder {
+	b.operations = append(b.operations, &CANIDBuilderOp{
+		kind: CANIDBuilderOpKindBitMask,
+		from: 0,
+		len:  11,
+	})
+	return b
+}
+
+// UseBitMask adds a bit mask operation from the given index and length.
+func (b *CANIDBuilder) UseBitMask(from, len int) {
+	b.operations = append(b.operations, &CANIDBuilderOp{
+		kind: CANIDBuilderOpKindBitMask,
+		from: from,
+		len:  len,
+	})
+}
diff --git a/canid_builder_test.go b/canid_builder_test.go
index 7d7e61d..34a4418 100644
--- a/canid_builder_test.go
+++ b/canid_builder_test.go
@@ -9,8 +9,8 @@ import (
 func Test_CANIDBuilder(t *testing.T) {
 	assert := assert.New(t)
 
-	b := NewCANIDBuilder("canid_builder")
-	b.UseMessagePriority(30).UseMessageID(4, 10).UseNodeID(0, 4)
+	b0 := NewCANIDBuilder("canid_builder_0")
+	b0.UseMessagePriority(30).UseMessageID(4, 10).UseNodeID(0, 4)
 
 	msgPriority := MessagePriorityLow
 	msgID := MessageID(0b1111111111)
@@ -20,7 +20,16 @@ func Test_CANIDBuilder(t *testing.T) {
 	expected |= uint32(msgID << 4)
 	expected |= uint32(nodeID)
 
-	res := b.Calculate(msgPriority, msgID, nodeID)
+	res0 := b0.Calculate(msgPriority, msgID, nodeID)
 
-	assert.Equal(expected, uint32(res))
+	assert.Equal(expected, uint32(res0))
+
+	b1 := NewCANIDBuilder("canid_builder_1")
+	b1.UseMessageID(4, 10).UseCAN2A()
+
+	expected = uint32(msgID<<4) & 0b11111111111
+
+	res1 := b1.Calculate(msgPriority, msgID, nodeID)
+
+	assert.Equal(expected, uint32(res1))
 }
diff --git a/utils.go b/utils.go
index f158a41..9f8a962 100644
--- a/utils.go
+++ b/utils.go
@@ -26,20 +26,21 @@ func CalculateBusLoad(bus *Bus, defCycleTime int) (float64, error) {
 
 	var headerBits int
 	var trailerBits int
-	var stuffingBits int
+	var headerStuffingBits int
 	switch bus.typ {
 	case BusTypeCAN2A:
 		// start of frame + id + rtr + ide + r0 + dlc
 		headerBits = 19
 		// crc + delim crc + slot ack + delim ack + eof
 		trailerBits = 25
-		// worst case scenario
-		stuffingBits = 19
+		// from wikipedia
+		headerStuffingBits = 34
 	}
 
 	consumedBitsPerSec := float64(0)
 	for _, tmpInt := range bus.nodeInts.getValues() {
 		for _, tmpMsg := range tmpInt.messages.getValues() {
+			stuffingBits := (headerStuffingBits + tmpMsg.sizeByte*8 - 1) / 4
 			msgBits := tmpMsg.sizeByte*8 + headerBits + trailerBits + stuffingBits
 
 			var cycleTime int
diff --git a/utils_test.go b/utils_test.go
index eb38b44..2f414a9 100644
--- a/utils_test.go
+++ b/utils_test.go
@@ -25,7 +25,8 @@ func Test_CalculateBusLoad(t *testing.T) {
 
 	load, err := CalculateBusLoad(bus, 500)
 	assert.NoError(err)
-	assert.Equal(10.16, load)
+	assert.Greater(load, 10.0)
+	assert.Less(load, 11.0)
 
 	argErr := &ArgumentError{}