mechutil 0.8.1

//
// Copyright (C) 2024 Automated Design Corp.. All Rights Reserved.
// Created Date: 2024-11-11 20:58:48
// -----
// Last Modified: 2024-11-13 08:38:37
// -----
//
//

//! MechFsmCommandArgTuple
//! The MechFsmCommandArgTuple attempts to reduce the number transmissions required to execute a command by
//! allowing for multiple arguments or varying types in the same transmission. To do so, values are serialized
//! into the data field, with separate arrays tracking type information and value length. The data field can
//! store up to 255 arguments, based upon argument length. For 255 values, every column would need to be a byte.
//! If every argument was four bytes long, the maximum number of arguments would be 63.
//!
//! Possible optimizations in the future:
//! - Reduce object size by eliminating type and length array, and embedding that information in the data array.
//! - - Extend data array size to 512, type code is 1 byte, length code is 1 byte.
//! - - Smaller transmission, more processing in the PLC and rust program to pull out individual columns.

use crate::register_value::MechCommandRegisterValue;
use crate::variant::{VariantTypeId, VariantValue};
use anyhow::anyhow;
use indexmap::IndexMap;
use num_traits::FromPrimitive;
use serde::{Deserialize, Serialize};
use serde_with::serde_as;

use serde::de::DeserializeOwned;
use std::convert::TryInto;
use std::mem::size_of; // Import DeserializeOwned for trait bound

//use serde_with::{serde_as, As};

/// Struct representing the S_MechFsmCommandArgTuple
#[repr(C)]
#[serde_as]
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct MechFsmCommandArgTuple {
    #[serde_as(as = "[_; 255]")]
    pub type_info: [VariantTypeId; 255], // Array of types for each column
    #[serde_as(as = "[_; 255]")]
    pub data: [u8; 255], // Serialized bytes of the value
    #[serde_as(as = "[_; 255]")]
    pub value_len: [u8; 255], // Length of each value
    pub num_columns: u8, // Number of columns in data
    pub num_rows: u8,    // Number of rows in data
}

impl MechFsmCommandArgTuple {
    /// Constructor
    pub fn new() -> Self {
        Self {
            type_info: [VariantTypeId::Null; 255],
            data: [0; 255],
            value_len: [0; 255],
            num_columns: 0,
            num_rows: 1,
        }
    }

    /// Serialize the structure to a byte array (1022 bytes)
    pub fn to_bytes(&self) -> [u8; 1022] {
        let mut ret = [0 as u8; 1022];

        // Serialize type_info (2 bytes per entry, 510 bytes total)
        for i in 0..255 {
            ret[i * 2..i * 2 + 2].copy_from_slice(&(self.type_info[i] as i16).to_le_bytes());
        }

        // Serialize data (255 bytes)
        ret[510..765].copy_from_slice(&self.data);

        // Serialize value_len (255 bytes)
        ret[765..1020].copy_from_slice(&self.value_len);

        // Serialize num_columns and num_rows (1 byte each)
        ret[1020] = self.num_columns;
        ret[1021] = self.num_rows;

        ret
    }

    /// Read the column value at the specified index as the specified type.
    /// The existing data should match the speicifed type, or at least be the same
    /// byte length. Returns an error if the column_index or T is invalid.
    pub fn read_column_value<T>(&self, column_index: u8) -> Result<T, anyhow::Error>
    where
        T: DeserializeOwned + Sized, // Require T to implement DeserializeOwned
    {
        if column_index >= self.num_columns {
            return Err(anyhow!(
                "Out of range! {} >= {}!",
                column_index,
                self.num_columns
            ));
        }

        // Check if the length of the data matches the size of the type T
        let expected_size = size_of::<T>();
        if self.value_len[column_index as usize] as usize != expected_size {
            return Err(anyhow!(
                "Size mismatch! Column size {} != expected size {}",
                self.value_len[column_index as usize],
                expected_size
            ));
        }

        // Calculate the starting index in `data` for the specified column
        let mut offset = 0;
        for i in 0..column_index {
            offset += self.value_len[i as usize] as usize;
        }

        // Extract the slice from data for the specific column
        let data_slice = &self.data[offset..offset + expected_size];

        // Try to convert the byte slice to the specified type T
        let value: T = postcard::from_bytes(data_slice).map_err(|e| {
            anyhow!(
                "Failed to deserialize data at column {}: {:?}",
                column_index,
                e
            )
        })?;

        Ok(value)
    }

    /// Convert the value at the specified column_index to a string. Returns an error if the
    /// column_index is invalid, or conversion isn't possible.
    pub fn to_string(&self, column_index: u8) -> Result<String, anyhow::Error> {
        match self.type_info[column_index as usize] {
            VariantTypeId::String => {
                // Find the start of the string.
                let mut offset = 0;
                for i in 0..column_index {
                    offset += self.value_len[i as usize] as usize;
                }

                // Find the end of the string
                let data_slice =
                    &self.data[offset..offset + self.value_len[column_index as usize] as usize];

                match String::from_utf8(data_slice.to_vec()) {
                    Ok(val) => return Ok(val),
                    Err(err) => {
                        return Err(anyhow!("Failed to deserialize string: {}", err));
                    }
                }
            }
            VariantTypeId::Bit => match self.read_column_value::<bool>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert Bit value to bool."));
                }
            },
            VariantTypeId::Byte => match self.read_column_value::<u8>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert Byte value to u8."));
                }
            },
            VariantTypeId::SByte => match self.read_column_value::<i8>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert SByte value to i8."));
                }
            },
            VariantTypeId::Int16 => match self.read_column_value::<i16>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert Int16 value to i16."));
                }
            },
            VariantTypeId::UInt16 => match self.read_column_value::<u16>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert UInt16 value to u16."));
                }
            },
            VariantTypeId::Int32 => match self.read_column_value::<i32>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert Int32 value to i32."));
                }
            },
            VariantTypeId::UInt32 => match self.read_column_value::<u32>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert UInt32 value to u32."));
                }
            },
            VariantTypeId::Int64 => match self.read_column_value::<i64>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert Int64 value to i64."));
                }
            },
            VariantTypeId::UInt64 => match self.read_column_value::<u64>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert UInt64 value to u64."));
                }
            },
            VariantTypeId::Real32 => match self.read_column_value::<f32>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert Real32 value to f32."));
                }
            },
            VariantTypeId::Real64 => match self.read_column_value::<f64>(column_index) {
                Ok(ret) => {
                    return Ok(ret.to_string());
                }
                _ => {
                    return Err(anyhow!("Failed to convert Real64 value to f64."));
                }
            },
            VariantTypeId::Null => Ok("<NULL>".to_string()),
            VariantTypeId::Array => Ok("[ARRAY]".to_string()),
            VariantTypeId::Object => Ok("{OBJECT}".to_string()),
        }
    }

    pub fn to_variant(&self, column_index: u8) -> Result<VariantValue, anyhow::Error> {
        match self.type_info[column_index as usize] {
            VariantTypeId::Null => {
                return Ok(VariantValue::Null);
            }
            VariantTypeId::Bit => match self.read_column_value::<bool>(column_index) {
                Ok(ret) => return Ok(VariantValue::Bit(ret)),
                Err(err) => return Err(anyhow!("Failed to conver to bool: {}", err)),
            },
            VariantTypeId::Byte => match self.read_column_value::<u8>(column_index) {
                Ok(ret) => return Ok(VariantValue::Byte(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to byte: {}", err)),
            },
            VariantTypeId::SByte => match self.read_column_value::<i8>(column_index) {
                Ok(ret) => return Ok(VariantValue::SByte(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to sbyte: {}", err)),
            },
            VariantTypeId::Int16 => match self.read_column_value::<i16>(column_index) {
                Ok(ret) => return Ok(VariantValue::Int16(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to int16: {}", err)),
            },
            VariantTypeId::UInt16 => match self.read_column_value::<u16>(column_index) {
                Ok(ret) => return Ok(VariantValue::UInt16(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to int16: {}", err)),
            },
            VariantTypeId::Int32 => match self.read_column_value::<i32>(column_index) {
                Ok(ret) => return Ok(VariantValue::Int32(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to int32: {}", err)),
            },
            VariantTypeId::UInt32 => match self.read_column_value::<u32>(column_index) {
                Ok(ret) => return Ok(VariantValue::UInt32(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to uint32: {}", err)),
            },
            VariantTypeId::Int64 => match self.read_column_value::<i64>(column_index) {
                Ok(ret) => return Ok(VariantValue::Int64(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to int64: {}", err)),
            },
            VariantTypeId::UInt64 => match self.read_column_value::<u64>(column_index) {
                Ok(ret) => return Ok(VariantValue::UInt64(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to uint64: {}", err)),
            },
            VariantTypeId::Real32 => match self.read_column_value::<f32>(column_index) {
                Ok(ret) => return Ok(VariantValue::Real32(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to real32: {}", err)),
            },
            VariantTypeId::Real64 => match self.read_column_value::<f64>(column_index) {
                Ok(ret) => return Ok(VariantValue::Real64(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to real64: {}", err)),
            },
            VariantTypeId::String => match self.to_string(column_index) {
                Ok(ret) => return Ok(VariantValue::String(ret)),
                Err(err) => return Err(anyhow!("Failed to convert to string: {}", err)),
            },
            VariantTypeId::Array => {
                return Err(anyhow!("Conversion to array not currently supported."));
            }
            VariantTypeId::Object => {
                return Err(anyhow!("Conversion to object not currently supported."));
            }
        }
    }

    /// Convert this instance of MechFsmCommandArgument to a MechCommandRegisterValue.
    /// A MechCommandRegisterValue can only hold one value, so only the first value
    /// is transferred. If there are no values contained in this MechFsmCommandArgument,
    /// then a NULL MechCommandRegisterValue is returned. If conversion is not possible,
    /// an error is returned.
    ///
    /// MechCommandRegisterValue is used as the backend for the object dictionary, so conversion
    /// is necessary when wanting to write a value to the Object Dictionary.
    pub fn to_register_value(&self) -> Result<MechCommandRegisterValue, anyhow::Error> {
        if self.num_columns == 0 || self.type_info[0] == VariantTypeId::Null {
            return Ok(MechCommandRegisterValue::new());
        }

        if self.type_info[0] == VariantTypeId::Array {
            return Err(anyhow!("ARRAY type not supported."));
        }

        if self.type_info[0] == VariantTypeId::Object {
            return Err(anyhow!("OBJECT type not supported."));
        }

        // Calculate the total length of the data
        let total_length = self.value_len[0] as usize;
        if total_length > 255 {
            return Err(anyhow!(
                "Data length exceeds maximum allowed size of 255 bytes. This MechFsmCommandArgument is in an invalid state."
            ));
        } else if total_length == 0 {
            return Err(anyhow!(
                "Data length of first column is 0. This MechFsmCommandArgument is in an invalid state."
            ));
        }

        // Initialize data array for MechCommandRegisterValue
        let mut register_data = [0u8; 255];
        register_data[..total_length].copy_from_slice(&self.data[..total_length]);

        Ok(MechCommandRegisterValue {
            type_id: self.type_info[0], // Assuming Array is the type for multi-column values
            value_len: total_length as u32, // Total length of serialized data
            num_columns: 1,             // Number of columns in this tuple
            num_rows: 1,                // Number of rows
            data: register_data,
        })
    }

    /// Calculate the total number of data bytes in the data field.
    /// This essentially returns the index of where to place the next
    /// data column.
    pub fn calculate_data_bytes(&self) -> usize {
        let mut ret = 0;
        for i in 0..self.num_columns {
            ret += self.value_len[i as usize] as usize;
        }

        return ret;
    }

    /// Clear out all arguments and data.
    pub fn clear(&mut self) {
        self.num_columns = 0;
        self.num_rows = 1;
        self.type_info = [VariantTypeId::Null; 255];
        self.data = [0; 255];
        self.value_len = [0; 255];
    }

    /// Push a bool value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_bool(&mut self, value: bool) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 1 < 255 {
            self.data[start_index] = value as u8;
            self.type_info[self.num_columns as usize] = VariantTypeId::Bit;
            self.value_len[self.num_columns as usize] = 1;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push a byte value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_byte(&mut self, value: u8) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 1 < 255 {
            self.data[start_index] = value;
            self.type_info[self.num_columns as usize] = VariantTypeId::Byte;
            self.value_len[self.num_columns as usize] = 1;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push a signed byte value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_sbyte(&mut self, value: i8) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 1 < 255 {
            self.data[start_index] = value as u8;
            self.type_info[self.num_columns as usize] = VariantTypeId::SByte;
            self.value_len[self.num_columns as usize] = 1;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push a u16 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_u16(&mut self, value: u16) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 2 < 255 {
            self.data[start_index..start_index + 2].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::UInt16;
            self.value_len[self.num_columns as usize] = 2;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push an i16 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_i16(&mut self, value: i16) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 2 < 255 {
            self.data[start_index..start_index + 2].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::Int16;
            self.value_len[self.num_columns as usize] = 2;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push a u32 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_u32(&mut self, value: u32) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 4 < 255 {
            self.data[start_index..start_index + 4].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::UInt32;
            self.value_len[self.num_columns as usize] = 4;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push a i32 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_i32(&mut self, value: i32) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 4 < 255 {
            self.data[start_index..start_index + 4].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::Int32;
            self.value_len[self.num_columns as usize] = 4;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push an f32 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_f32(&mut self, value: f32) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 4 < 255 {
            self.data[start_index..start_index + 4].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::Real32;
            self.value_len[self.num_columns as usize] = 4;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push a u64 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_u64(&mut self, value: u64) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 8 < 255 {
            self.data[start_index..start_index + 8].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::UInt64;
            self.value_len[self.num_columns as usize] = 8;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push an i64 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_i64(&mut self, value: i64) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 8 < 255 {
            self.data[start_index..start_index + 8].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::Int64;
            self.value_len[self.num_columns as usize] = 8;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push an f64 value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_f64(&mut self, value: f64) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        if start_index + 8 < 255 {
            self.data[start_index..start_index + 8].copy_from_slice(&value.to_le_bytes());
            self.type_info[self.num_columns as usize] = VariantTypeId::Real64;
            self.value_len[self.num_columns as usize] = 8;
            self.num_columns += 1;
            return Ok(());
        } else {
            return Err(anyhow!(
                "Out of capacity in data field for additional value."
            ));
        }
    }

    /// Push a string value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    pub fn push_value_string(&mut self, value: &str) -> Result<(), anyhow::Error> {
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        let start_index = self.calculate_data_bytes();
        let str_len = value.len();

        if str_len > 254 {
            return Err(anyhow!(
                "String length of {} not allowed. Must be less than 254 chars.",
                str_len
            ));
        }

        if str_len > 0 {
            if start_index + str_len < 255 {
                self.data[start_index..start_index + str_len].copy_from_slice(&value.as_bytes());
                self.type_info[self.num_columns as usize] = VariantTypeId::String;
                self.value_len[self.num_columns as usize] = str_len as u8;
                self.num_columns += 1;
                return Ok(());
            } else {
                return Err(anyhow!(
                    "Out of capacity in data field for additional string of {} chars.",
                    str_len
                ));
            }
        } else {
            if start_index + 1 < 255 {
                self.data[start_index] = 0; // null value place holder
                self.type_info[self.num_columns as usize] = VariantTypeId::String;
                self.value_len[self.num_columns as usize] = 1;
                self.num_columns += 1;
                return Ok(());
            } else {
                return Err(anyhow!(
                    "Out of capacity in data field for additional string of {} chars.",
                    str_len
                ));
            }
        }
    }

    /// Push an MechCommandRegisterValue value to the end of the argument list. Returns Ok if successful,
    /// anyhow::Error if there is not capacity for this additional argument.
    /// This function is usually used when returning a value from the object dictionary.
    pub fn push_value_register(
        &mut self,
        value: &MechCommandRegisterValue,
    ) -> Result<(), anyhow::Error> {
        // Check if there is enough capacity for additional data
        let total_length = self.calculate_data_bytes();
        if total_length + value.value_len as usize > 255 {
            return Err(anyhow!(
                "Data length exceeds maximum allowed size of 255 bytes"
            ));
        }

        // Check if num_columns will exceed the allowed limit of 255 columns
        if self.num_columns >= 255 {
            return Err(anyhow!("Too many columns! Out of capacity."));
        }

        // Copy value's data to self.data at the current end position
        let start_index = total_length;
        self.data[start_index..start_index + value.value_len as usize]
            .copy_from_slice(&value.data[..value.value_len as usize]);

        // Set type_info, value_len, num_columns, and num_rows
        self.type_info[self.num_columns as usize] = value.type_id;
        self.value_len[self.num_columns as usize] = value.value_len as u8; // assuming max value_len is 255
        self.num_columns += value.num_columns as u8; // increment by the number of columns in the register value

        Ok(())
    }
}

// Conversion to VariantValue. The VariantValue is used by our communication pipelines to represent data
// in the controller. The communications client will give us a VariantValue, and it needs to be converted
// into the MechFsmCommandArgTuple.
impl TryInto<VariantValue> for MechFsmCommandArgTuple {
    type Error = anyhow::Error;

    fn try_into(self) -> Result<VariantValue, anyhow::Error> {
        let mut map = IndexMap::new();

        let type_info_data: Vec<VariantValue> = self
            .type_info
            .iter()
            .map(|&t| VariantValue::Int16(t as i16))
            .collect();

        println!("type_info_data: {:?}", type_info_data);

        let data_data: Vec<VariantValue> =
            self.data.iter().map(|&d| VariantValue::Byte(d)).collect();
        let value_len_data: Vec<VariantValue> = self
            .value_len
            .iter()
            .map(|&v| VariantValue::Byte(v))
            .collect();

        map.insert("type_info".to_string(), VariantValue::Array(type_info_data));
        map.insert("data".to_string(), VariantValue::Array(data_data));
        map.insert("value_len".to_string(), VariantValue::Array(value_len_data));
        map.insert(
            "num_columns".to_string(),
            VariantValue::Byte(self.num_columns),
        );
        map.insert("num_rows".to_string(), VariantValue::Byte(self.num_rows));

        Ok(VariantValue::Object(Box::new(map)))
    }
}

// Conversion from VariantValue
impl TryFrom<VariantValue> for MechFsmCommandArgTuple {
    type Error = anyhow::Error;

    fn try_from(value: VariantValue) -> Result<Self, Self::Error> {
        match value {
            VariantValue::Object(map) => {
                let type_info = map
                    .get("type_info")
                    .and_then(|v| v.to_array().ok())
                    .ok_or_else(|| anyhow!("Invalid type_info field"))?
                    .into_iter()
                    .map(|v| {
                        VariantTypeId::from_i16(v.to_int16().unwrap_or(0))
                            .unwrap_or(VariantTypeId::Null)
                    })
                    .collect::<Vec<VariantTypeId>>()
                    .try_into()
                    .unwrap_or([VariantTypeId::Null; 255]);

                let data = map
                    .get("data")
                    .and_then(|v| v.to_array().ok())
                    .ok_or_else(|| anyhow!("Invalid data field"))?
                    .into_iter()
                    .map(|v| v.to_byte().unwrap_or(0))
                    .collect::<Vec<u8>>()
                    .try_into()
                    .unwrap_or([0; 255]);

                let value_len = map
                    .get("value_len")
                    .and_then(|v| v.to_array().ok())
                    .ok_or_else(|| anyhow!("Invalid value_len field"))?
                    .into_iter()
                    .map(|v| v.to_byte().unwrap_or(0))
                    .collect::<Vec<u8>>()
                    .try_into()
                    .unwrap_or([0; 255]);

                let num_columns = map
                    .get("num_columns")
                    .and_then(|v| v.to_byte().ok())
                    .unwrap_or(0);

                let num_rows = map
                    .get("num_rows")
                    .and_then(|v| v.to_byte().ok())
                    .unwrap_or(1);

                Ok(Self {
                    type_info,
                    data,
                    value_len,
                    num_columns,
                    num_rows,
                })
            }
            _ => Err(anyhow!("VariantValue is not of Object type")),
        }
    }
}

#[test]
fn command_arg_to_bytes() {
    let mut arg = MechFsmCommandArgTuple::new();
    let _ = arg.push_value_f32(-293.7);
    let _ = arg.push_value_f32(-9.9);
    let _ = arg.push_value_f32(500.3);
    let _ = arg.push_value_f32(180.0);
    let _ = arg.push_value_f32(0.0);
    let _ = arg.push_value_f32(0.0);
    let _ = arg.push_value_u16(1234);
    let _ = arg.push_value_bool(true);

    println!("arg: {:?}", arg);

    let bytes = arg.to_bytes();

    println!("BYTES: {:?}", bytes);

    for i in 510..527 {
        println!("bytes[{}] = {}", i, bytes[i]);
    }

    for i in 765..775 {
        println!("bytes[{}] = {}", i, bytes[i]);
    }

    println!("bytes[1020] = {}", bytes[1020]);
    println!("bytes[1021] = {}", bytes[1021]);
    // println!("bytes[1022] = {}", bytes[1022]);

    // Serialize the struct without the crc field
    use crc32fast::Hasher;
    let mut hasher = Hasher::new();

    hasher.update(&bytes);
    let crc32 = hasher.finalize();

    println!("CRC32 = {}", crc32);

    assert_eq!(bytes[1020], 8);
    assert_eq!(bytes[1021], 1);
    assert_eq!(bytes[2], 10);
    assert_eq!(bytes[510], 154);
    assert_eq!(bytes[515], 102);
    assert_eq!(crc32, 2242037589);
}