using System;
using System.Linq;
using System.Collections.Generic;


public static class CSAFEDictionary
{
    public static readonly byte ExtendedFrameStartFlag = 0xF0;
    public static readonly byte StandardFrameStartFlag = 0xF1;
    public static readonly byte StopFrameFlag = 0xF2;
    public static readonly byte ByteStuffingFlag = 0xF3;

    public static readonly Dictionary<string, List<object>> Cmd = new Dictionary<string, List<object>>
    {
        // Standard Short Commands
        { "CSAFE_GETSTATUS_CMD",           new List<object> { 0x80, new int[] { } } },
        { "CSAFE_RESET_CMD",               new List<object> { 0x81, new int[] { } } },
        { "CSAFE_GOIDLE_CMD",              new List<object> { 0x82, new int[] { } } },
        { "CSAFE_GOHAVEID_CMD",            new List<object> { 0x83, new int[] { } } },
        { "CSAFE_GOINUSE_CMD",             new List<object> { 0x85, new int[] { } } },
        { "CSAFE_GOFINISHED_CMD",          new List<object> { 0x86, new int[] { } } },
        { "CSAFE_GOREADY_CMD",             new List<object> { 0x87, new int[] { } } },
        { "CSAFE_BADID_CMD",               new List<object> { 0x88, new int[] { } } },
        { "CSAFE_GETVERSION_CMD",          new List<object> { 0x91, new int[] { } } },
        { "CSAFE_GETID_CMD",               new List<object> { 0x92, new int[] { } } },
        { "CSAFE_GETUNITS_CMD",            new List<object> { 0x93, new int[] { } } },
        { "CSAFE_GETSERIAL_CMD",           new List<object> { 0x94, new int[] { } } },
        { "CSAFE_GETODOMETER_CMD",         new List<object> { 0x9B, new int[] { } } },
        { "CSAFE_GETERRORCODE_CMD",        new List<object> { 0x9C, new int[] { } } },
        { "CSAFE_GETTWORK_CMD",            new List<object> { 0xA0, new int[] { } } },
        { "CSAFE_GETHORIZONTAL_CMD",       new List<object> { 0xA1, new int[] { } } },
        { "CSAFE_GETCALORIES_CMD",         new List<object> { 0xA3, new int[] { } } },
        { "CSAFE_GETPROGRAM_CMD",          new List<object> { 0xA4, new int[] { } } },
        { "CSAFE_GETPACE_CMD",             new List<object> { 0xA6, new int[] { } } },
        { "CSAFE_GETCADENCE_CMD",          new List<object> { 0xA7, new int[] { } } },
        { "CSAFE_GETUSERINFO_CMD",         new List<object> { 0xAB, new int[] { } } },
        { "CSAFE_GETHRCUR_CMD",            new List<object> { 0xB0, new int[] { } } },
        { "CSAFE_GETPOWER_CMD",            new List<object> { 0xB4, new int[] { } } },
        
        // Configuration / Set Commands
        { "CSAFE_AUTOUPLOAD_CMD",          new List<object> { 0x01, new int[] { 1 } } },
        { "CSAFE_IDDIGITS_CMD",            new List<object> { 0x10, new int[] { 1 } } },
        { "CSAFE_SETTIME_CMD",             new List<object> { 0x11, new int[] { 1, 1, 1 } } },
        { "CSAFE_SETDATE_CMD",             new List<object> { 0x12, new int[] { 1, 1, 1 } } },
        { "CSAFE_SETTIMEOUT_CMD",          new List<object> { 0x13, new int[] { 1 } } },
        { "CSAFE_SETUSERCFG1_CMD",         new List<object> { 0x1A, new int[] { 0 } } },
        { "CSAFE_SETTWORK_CMD",            new List<object> { 0x20, new int[] { 1, 1, 1 } } },
        { "CSAFE_SETHORIZONTAL_CMD",       new List<object> { 0x21, new int[] { 2, 1 } } },
        { "CSAFE_SETCALORIES_CMD",         new List<object> { 0x23, new int[] { 2 } } },
        { "CSAFE_SETPROGRAM_CMD",          new List<object> { 0x24, new int[] { 1, 1 } } },
        { "CSAFE_SETPOWER_CMD",            new List<object> { 0x34, new int[] { 2, 1 } } },
        { "CSAFE_GETCAPS_CMD",             new List<object> { 0x70, new int[] { 1 } } },
        
        // PM5 Specific Long Commands (Wrapper 0x1A)
        { "CSAFE_PM_GET_WORKOUTTYPE",      new List<object> { 0x89, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_DRAGFACTOR",       new List<object> { 0xC1, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_STROKESTATE",      new List<object> { 0xBF, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_WORKTIME",         new List<object> { 0xA0, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_WORKDISTANCE",     new List<object> { 0xA3, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_ERRORVALUE",       new List<object> { 0xC9, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_WORKOUTSTATE",     new List<object> { 0x8D, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_WORKOUTINTERVALCOUNT", new List<object> { 0x9F, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_INTERVALTYPE",     new List<object> { 0x8E, new int[] { }, 0x1A } },
        { "CSAFE_PM_GET_RESTTIME",         new List<object> { 0xCF, new int[] { }, 0x1A } },
        { "CSAFE_PM_SET_SPLITDURATION",    new List<object> { 0x05, new int[] { 1, 4 }, 0x1A } }, // Fixed ID
        { "CSAFE_PM_GET_FORCEPLOTDATA",    new List<object> { 0x6B, new int[] { 1 }, 0x1A } },    // Fixed to 0x6B, 1 arg (block size)
        { "CSAFE_PM_SET_SCREENERRORMODE",  new List<object> { 0x27, new int[] { 1 }, 0x1A } },
        { "CSAFE_PM_GET_HEARTBEATDATA",    new List<object> { 0x6C, new int[] { }, 0x1A } }       // FIXED: No arguments required
    };

    public static readonly Dictionary<int, List<object>> Resp = new Dictionary<int, List<object>>
    {
        { 0x80, new List<object> { "CSAFE_GETSTATUS_CMD",                   new int[] { 0 } } },
        { 0x81, new List<object> { "CSAFE_RESET_CMD",                       new int[] { 0 } } },
        { 0x82, new List<object> { "CSAFE_GOIDLE_CMD",                      new int[] { 0 } } },
        { 0x83, new List<object> { "CSAFE_GOHAVEID_CMD",                    new int[] { 0 } } },
        { 0x85, new List<object> { "CSAFE_GOINUSE_CMD",                     new int[] { 0 } } },
        { 0x86, new List<object> { "CSAFE_GOFINISHED_CMD",                  new int[] { 0 } } },
        { 0x87, new List<object> { "CSAFE_GOREADY_CMD",                     new int[] { 0 } } },
        { 0x88, new List<object> { "CSAFE_BADID_CMD",                       new int[] { 0 } } },
        { 0x91, new List<object> { "CSAFE_GETVERSION_CMD",                  new int[] { 1, 1, 1, 2, 2 } } }, 
        { 0x92, new List<object> { "CSAFE_GETID_CMD",                       new int[] { -5 } } },            
        { 0x93, new List<object> { "CSAFE_GETUNITS_CMD",                    new int[] { 1 } } },             
        { 0x94, new List<object> { "CSAFE_GETSERIAL_CMD",                   new int[] { -9 } } },            
        { 0x9B, new List<object> { "CSAFE_GETODOMETER_CMD",                 new int[] { 4, 1 } } },          
        { 0x9C, new List<object> { "CSAFE_GETERRORCODE_CMD",                new int[] { 3 } } },             
        { 0xA0, new List<object> { "CSAFE_GETTWORK_CMD",                    new int[] { 1, 1, 1 } } },       
        { 0xA1, new List<object> { "CSAFE_GETHORIZONTAL_CMD",               new int[] { 2, 1 } } },          
        { 0xA3, new List<object> { "CSAFE_GETCALORIES_CMD",                 new int[] { 2 } } },             
        { 0xA4, new List<object> { "CSAFE_GETPROGRAM_CMD",                  new int[] { 1 } } },             
        { 0xA6, new List<object> { "CSAFE_GETPACE_CMD",                     new int[] { 2, 1 } } },          
        { 0xA7, new List<object> { "CSAFE_GETCADENCE_CMD",                  new int[] { 2, 1 } } },          
        { 0xAB, new List<object> { "CSAFE_GETUSERINFO_CMD",                 new int[] { 2, 1, 1, 1 } } },    
        { 0xB0, new List<object> { "CSAFE_GETHRCUR_CMD",                    new int[] { 1 } } },             
        { 0xB4, new List<object> { "CSAFE_GETPOWER_CMD",                    new int[] { 2, 1 } } },          
        { 0x01, new List<object> { "CSAFE_AUTOUPLOAD_CMD",                  new int[] { 0 } } },
        { 0x10, new List<object> { "CSAFE_IDDIGITS_CMD",                    new int[] { 0 } } },
        { 0x11, new List<object> { "CSAFE_SETTIME_CMD",                     new int[] { 0 } } },
        { 0x12, new List<object> { "CSAFE_SETDATE_CMD",                     new int[] { 0 } } },
        { 0x13, new List<object> { "CSAFE_SETTIMEOUT_CMD",                  new int[] { 0 } } },
        { 0x1A, new List<object> { "CSAFE_SETUSERCFG1_CMD",                 new int[] { 0 } } },             
        { 0x20, new List<object> { "CSAFE_SETTWORK_CMD",                    new int[] { 0 } } },
        { 0x21, new List<object> { "CSAFE_SETHORIZONTAL_CMD",               new int[] { 0 } } },
        { 0x23, new List<object> { "CSAFE_SETCALORIES_CMD",                 new int[] { 0 } } },
        { 0x24, new List<object> { "CSAFE_SETPROGRAM_CMD",                  new int[] { 0 } } },
        { 0x34, new List<object> { "CSAFE_SETPOWER_CMD",                    new int[] { 0 } } },
        { 0x70, new List<object> { "CSAFE_GETCAPS_CMD",                     new int[] { 11 } } },            
        
        { 0x1A89, new List<object> { "CSAFE_PM_GET_WORKOUTTYPE",            new int[] { 1 } } },             
        { 0x1AC1, new List<object> { "CSAFE_PM_GET_DRAGFACTOR",             new int[] { 1 } } },             
        { 0x1ABF, new List<object> { "CSAFE_PM_GET_STROKESTATE",            new int[] { 1 } } },             
        { 0x1AA0, new List<object> { "CSAFE_PM_GET_WORKTIME",               new int[] { 4, 1 } } },          
        { 0x1AA3, new List<object> { "CSAFE_PM_GET_WORKDISTANCE",           new int[] { 4, 1 } } },          
        { 0x1AC9, new List<object> { "CSAFE_PM_GET_ERRORVALUE",             new int[] { 2 } } },             
        { 0x1A8D, new List<object> { "CSAFE_PM_GET_WORKOUTSTATE",           new int[] { 1 } } },             
        { 0x1A9F, new List<object> { "CSAFE_PM_GET_WORKOUTINTERVALCOUNT",   new int[] { 1 } } },             
        { 0x1A8E, new List<object> { "CSAFE_PM_GET_INTERVALTYPE",           new int[] { 1 } } },             
        { 0x1ACF, new List<object> { "CSAFE_PM_GET_RESTTIME",               new int[] { 2 } } },             
        { 0x1A05, new List<object> { "CSAFE_PM_SET_SPLITDURATION",          new int[] { 0 } } },             
        { 0x1A6B, new List<object> { "CSAFE_PM_GET_FORCEPLOTDATA",          new int[] { -1 } } }, 
        { 0x1A27, new List<object> { "CSAFE_PM_SET_SCREENERRORMODE",        new int[] { 0 } } },             
        { 0x1A6C, new List<object> { "CSAFE_PM_GET_HEARTBEATDATA",          new int[] { -1 } } }
    };
}

public static class CSAFECommand
{
    public static List<byte> Write(string[] arguments)
    {
        int i = 0;
        List<byte> message = new List<byte>();
        List<byte> wrapper = new List<byte>();
        int wrapped = 0;

        while (i < arguments.Length)
        {
            string argument = arguments[i];
            int commandPropertiesIdInt = Convert.ToInt32(CSAFEDictionary.Cmd[argument][0]);
            int[] cmdProps = CSAFEDictionary.Cmd[argument][1] as int[];
            
            int wrapperInt = 0;
            if (CSAFEDictionary.Cmd[argument].Count > 2)
                wrapperInt = Convert.ToInt32(CSAFEDictionary.Cmd[argument][2]);

            List<byte> command = new List<byte>();

            if (cmdProps.Length != 0)
            {
                for (int bytes = 0; bytes < cmdProps.Length; bytes++)
                {
                    i++;
                    int intValue = int.Parse(arguments[i]);
                    byte[] value = IntToBytes(intValue, cmdProps[bytes]);
                    command.AddRange(value);
                }
                command.Insert(0, Convert.ToByte(command.Count));
            }

            command.Insert(0, Convert.ToByte(commandPropertiesIdInt));

            if (wrapperInt == 0x1A)
            {
                if (wrapped == 0)
                {
                    wrapper.Insert(0, Convert.ToByte(wrapperInt));
                    wrapped = 1;
                }
                wrapper.AddRange(command);
            }
            else
            {
                message.AddRange(command);
            }
            i++;
        }

        if (wrapped == 1)
        {
            wrapper.Insert(1, Convert.ToByte(wrapper.Count - 1));
            message.AddRange(wrapper);
        }

        int checksum = 0;
        for (int j = 0; j < message.Count; j++)
        {
            checksum ^= message[j];
        }

        int k = 0;
        while (k < message.Count)
        {
            if (0xF0 <= message[k] && message[k] <= 0xF3)
            {
                byte stuffValue = Convert.ToByte(message[k] & 0x03);
                message[k] = CSAFEDictionary.ByteStuffingFlag; 
                message.Insert(k + 1, stuffValue);
                k++; 
            }
            k++;
        }

        if (0xF0 <= checksum && checksum <= 0xF3)
        {
            message.Add(CSAFEDictionary.ByteStuffingFlag);
            message.Add(Convert.ToByte(checksum & 0x03));
        }
        else
        {
            message.Add(Convert.ToByte(checksum));
        }

        message.Insert(0, CSAFEDictionary.StandardFrameStartFlag);
        message.Add(CSAFEDictionary.StopFrameFlag);

        return message;
    }

public static Dictionary<string, List<string>> Read(byte[] transmission)
    {
        Dictionary<string, List<string>> response = new Dictionary<string, List<string>>();
        List<byte> message = transmission.ToList();

        int startFlagIndex = message.IndexOf(CSAFEDictionary.StandardFrameStartFlag);
        int extStartFlagIndex = message.IndexOf(CSAFEDictionary.ExtendedFrameStartFlag);
        int startIndex = Math.Max(startFlagIndex, extStartFlagIndex);
        int stopFlagIndex = message.IndexOf(CSAFEDictionary.StopFrameFlag);

        if (startIndex == -1 || stopFlagIndex == -1 || stopFlagIndex < startIndex)
            return response; 

        message = message.GetRange(startIndex + 1, stopFlagIndex - startIndex - 1);
        message = CheckMessage(message);
        
        if (message == null || message.Count == 0) return response; 

        int k = 0;
        int msgState = message[k];
        response["CSAFE_GETSTATUS_CMD"] = new List<string> { msgState.ToString() };
        k++;

        // NEW: Track the end index of the PM5 wrapper block
        int wrapperEndIndex = -1; 

        while (k < message.Count)
        {
            try 
            {
                int k_before = k; // Track where this specific command started
                int responseId = message[k];
                k++;

                // Wrapper Check
                if (responseId == 0x1A)
                {
                    if (k >= message.Count) break; 
                    int wrapperLength = message[k];
                    k++; 

                    if (wrapperLength > 0)
                    {
                        wrapperEndIndex = k + wrapperLength; // Mark the boundary of wrapped data
                        
                        if (k < message.Count)
                        {
                            responseId = message[k];
                            responseId = responseId | (0x1A << 8);
                            k++;
                        }
                    }
                    else
                    {
                        continue; 
                    }
                }
                // NEW: If we are still reading bytes that fall inside the wrapper length,
                // automatically apply the 0x1A PM5 prefix!
                else if (k_before < wrapperEndIndex) 
                {
                    responseId = responseId | (0x1A << 8);
                }

                if (!CSAFEDictionary.Resp.ContainsKey(responseId))
                {
                    if (k < message.Count) k += message[k] + 1;
                    continue; 
                }

                List<object> commandResponse = CSAFEDictionary.Resp[responseId];
                string commandName = commandResponse[0] as string;
                int[] commandByteCountArray = commandResponse[1] as int[];
                
                List<string> result = new List<string>();

                // SAFEGUARD: Prevent out of bounds
                if (k >= message.Count) break; 
                int payloadLength = message[k]; 
                k++;

                int bytesReadForCmd = 0;

                foreach (int commandByteCount in commandByteCountArray)
                {
                    if (bytesReadForCmd >= payloadLength) break; 

                    int bytesToRead = commandByteCount >= 0 ? commandByteCount : (payloadLength - bytesReadForCmd);
                    
                    if (k + bytesToRead > message.Count) break; 

                    byte[] rawBytes = message.GetRange(k, bytesToRead).ToArray();
                    string value = "";

                    if (commandByteCount >= 0)
                    {
                        int intValue = 0;
                        for (int b = 0; b < rawBytes.Length; b++)
                        {
                            intValue |= (rawBytes[b] << (8 * b));
                        }
                        value = intValue.ToString();
                    }
                    else
                    {
                        value = string.Join(",", rawBytes.Select(b => b.ToString()));
                    }

                    result.Add(value);
                    k += bytesToRead;
                    bytesReadForCmd += bytesToRead;
                }

                if (bytesReadForCmd < payloadLength)
                {
                    k += (payloadLength - bytesReadForCmd);
                }

                response[commandName] = result;
            }
            catch (Exception)
            {
                break;
            }
        }

        return response;
    }

    private static List<byte> CheckMessage(List<byte> message)
    {
        int i = 0;
        int checksum = 0;

        while (i < message.Count)
        {
            if (message[i] == CSAFEDictionary.ByteStuffingFlag)
            {
                if (i + 1 >= message.Count) break; 
                byte stuffValue = message[i + 1]; 
                message.RemoveAt(i + 1);
                message[i] = Convert.ToByte(0xF0 | stuffValue);
            }

            checksum ^= message[i];
            i++;
        }

        if (checksum != 0) return new List<byte>();

        if (message.Count > 0) message.RemoveAt(message.Count - 1);
        return message;
    }

    private static byte[] IntToBytes(int value, int length)
    {
        byte[] result = new byte[length];
        for (int i = 0; i < length; i++)
        {
            result[i] = (byte)((value >> (i * 8)) & 0xFF);
        }
        return result;
    }
}