File content as of revision 1:362331cec9b7:

#include "mbed.h"
#include "cmdmsg.h"
#include "ledstrip.h"
//#define DETECT_LEDS
#ifdef DETECT_LEDS
#include "detectled.h"
#endif
//#define USE_SPIDEY_GEOM
#ifdef USE_SPIDEY_GEOM
#include "spideyGeometry.h"
#endif
/*

Message format is:
    <SEQ><CMD><CMD><CMD> ... <ZERO>
    
Where:
    <SEQ> is a two byte sequence count
    <CMD> is a series of bytes as follows:
        <LEN><COD><PARAMS>
        
Where:
    <LEN> is a two byte with value 1..65535 depicting msg len
    <COD> is a one byte command code - see below
    <PARAMS> are a series of bytes containing the parameters for the command
    
<COD> values:

    NOTE MSB of COD is used to indicate that no response is required on success
         i.e. if MSB of COD is 1 then no response is required on success
         
    (COD & 0x7f) ....
    0x01:
        Clear the entire strip
    0x02:
        Fill a series of leds with RGB values
        <PARAMS> := <START><NUM><RGB1><RGB2>
        Where:
            <START> is a two byte value (bigendian) for led to start series
            <NUM> is a two byte (bigendian) number of leds to set
            <RGB1> is the starting RGB value (24bits)
            <RGB2> is the ending RGB value (24bits) - OPTIONAL
        The RGB2 value is optional - the LEN value determines whether it is present or not
        - if present the colour values are interpolated (HSV) between RGB1 and RGB2
        - if not present all leds are set to colour RGB1
    0x03:
        Detect LED with opto-sensor (put opto-sensor on LED and run command)
        Sensor must be attached to p20 - currently using BPX65 with 470K pullup
        Returns string with LED number
    0x04:
        Set number of leds and split point
        <PARAMS> := <NUMLEDS><SPLITPOINT>
        Where:
            <NUMLEDS> is a two byte (bigendian) value for number of leds in strip
            <SPLITPOINT> is a two byte (bigendian) value for point at which DigitalOut(p10) is used instead of p12 for data
                    (this is because the spidey wall is two parallel chains of LEDS)
    0x05:
        Raw RGB values for a series of leds
        <PARAMS> := <START><NUM><RGB1><RGB2><RGB2>....<RGBn>
        Where:
            <START> is a two byte value (bigendian) for led to start series
            <NUM> is a two byte (bigendian) number of leds to set
            <RGB1> is the RGB value for 1st LED (24bits)
            <RGB2> is the RGB value for 2nd LED (24bits)
            <RGBn> is the RGB value for nth LED (24bits)
    0x06:
        Set colour for leds on a pad
        <PARAMS> := <PAD><RGB1><RGB2>
        Where:
            <PAD> is the pad number (1 byte)
            <RGB1> is the starting RGB value (24bits)
            <RGB2> is the ending RGB value (24bits) - OPTIONAL
        The RGB2 value is optional - the LEN value determines whether it is present or not
        - if present the colour values are interpolated (HSV) between RGB1 and RGB2
        - if not present all leds are set to colour RGB1
    0x07:
        Set node and link info
        <PARAMS> := <NODE><NODEFLAGS><NODERGB><LINKIDX><LINKLEDSTART><LINKNUMLEDS><LINKRGB>
        Where:
            <NODE> is the node number (1 byte)
            <NODEFLAGS> LSB is 0 if the RGB value for the node is not to be changed, or 1 otherwise (1 byte)
            <NODERGB> is the RBG value for the leds in the node itself (3 bytes)
            <LINKIDX> is the index of the link (edge) from this node to control (1 byte)
            <LINKLEDSTART> is the LED along the link to start controlling (1 byte)
            <LINKNUMLEDS> is the number of LEDs to control (1 byte)
            <LINKRGB> is the colour to make the link LEDs (3 bytes)
        
*/

#ifdef USE_SPIDEY_GEOM
int cmdmsg::getLedNumFromSeq(const int* edgeLeds, int numLedsInSeq, int seqIdx)
{
    if (seqIdx >= numLedsInSeq)
        return -1;
    int curSeqPos = 0;
    bool reverseSeq = false;
    for (int i = 0; i < MAX_SPIDEY_LINK_ELS/2; i++)
    {
        int startLedIdx = edgeLeds[i * 2];
        int endLedIdx = edgeLeds[i * 2 + 1];
        int nLeds = endLedIdx - startLedIdx + 1;
        if (startLedIdx > endLedIdx)
        {
            reverseSeq = true;
            nLeds = startLedIdx - endLedIdx + 1;
        }
        if (seqIdx - curSeqPos < nLeds)
        {
            if (reverseSeq)
                return startLedIdx - seqIdx;
            return startLedIdx + seqIdx;
        }
        curSeqPos = nLeds;
    }
    return -1;
}
#endif

char* cmdmsg::Interpret(const unsigned char* msg, int msgLen, ledstrip* pLedStrip)
{
    // Get message details
    static char responseStr[50];
    unsigned char* ledStrip = pLedStrip->GetBuffer();
    int nBytesLeft = msgLen - 2;
    int seqCount = (msg[0] * 256) + msg[1];
    msg += 2;
    while (nBytesLeft > 0)
    {
        // Length of chunk
        int chunkLen = (msg[0] * 256) + msg[1];
        if (chunkLen == 0)
            break;
        
        // Params length
        int paramsLen = chunkLen - 1;
//        printf("Chunklen = %d, Paramslen = %d\r\n", chunkLen, paramsLen);
        
        // Command and whether response required on success
        int cmdCode = msg[2];
        if ((cmdCode & 0x80) == 0)
            sprintf(responseStr, "%04x OK", seqCount);
        else
            strcpy(responseStr, "");
        
        // Move message pointer to assist in decoding the payload
        msg += 3;
        switch(cmdCode & 0x7f)
        {
            case 0x01: // Clear
            {
//                printf("Clear - nextbyte %d\r\n", msg[0]);
                pLedStrip->Clear();
                break;
            }
            case 0x02: // Fill
            {
//                printf("Fill\r\n");

                int startLed = (msg[0] * 256) + msg[1];
                int numLeds = (msg[2] * 256) + msg[3];
//                printf("F Q%04x S%d N%d Pa%d Ln%d\r\n", seqCount, startLed, numLeds, paramsLen, msgLen);
                if (paramsLen == 10)  // RGB2 is provided
                {
                    pLedStrip->Fill(startLed, numLeds, msg[4], msg[5], msg[6], msg[7], msg[8], msg[9]);
                } else if (paramsLen == 7) // only RGB1 - so solid colour fill
                {
                    pLedStrip->Fill(startLed, numLeds, msg[4], msg[5], msg[6]);
                }
                break;
            }
            case 0x03: // Detect LED
            {
#ifdef DETECT_LEDS
                detectled ledDetector(p20);
                int selectedLed = ledDetector.DetectSelectedLed(pLedStrip);
                sprintf(responseStr, "%02x OK LED %d", seqCount, selectedLed);
#endif
                break;
            }
            case 0x04: // Set numleds and splitpoint
            {
//                printf("Set numleds and splitpoint\r\n");

                if (paramsLen == 4)
                {
                    int numLeds = (msg[0] * 256) + msg[1];
                    int splitPoint = (msg[2] * 256) + msg[3];
                    pLedStrip->Resize(numLeds, splitPoint);
                }
                sprintf(responseStr, "%04x OK Resized", seqCount);
                break;
            }
            case 0x05: // Raw - set LEDs to RGB values from buffer
            {
                if (paramsLen > 4)
                {
                    int startLed = (msg[0] * 256) + msg[1];
                    int numLeds = (msg[2] * 256) + msg[3];
                    if (paramsLen >= numLeds * 3 + 4)
                    {
                        pLedStrip->RawFill(startLed, numLeds, msg+4);
//                        printf("R Q%04x S%d N%d Pa%d Ln%d\r\n", seqCount, startLed, numLeds, paramsLen, mMsgLen);
                    }
                    else
                    {
                        sprintf(responseStr, "%04x RAW FILL - BufLengthMismatch %d", seqCount, paramsLen);
                    }
                }
                else
                {
                    sprintf(responseStr, "%04x RAW FILL - ParamsLen Error %d", seqCount, paramsLen);            
                }
                break;
            }
            case 0x06: // Set pad leds to a colour
            {
#ifdef USE_SPIDEY_GEOM
                printf("Pad\r\n");

                if (paramsLen > 2)
                {
                    int padIdx = msg[0];
                    if (padIdx < sizeof(_spideyPads)/sizeof(SpideyPadInfo))
                    {
                        int firstLed = _spideyPads[padIdx].padLeds[0];
                        int numLeds = _spideyPads[padIdx].padLeds[1] - _spideyPads[padIdx].padLeds[0] + 1;
                        if (paramsLen == 4) // Only 1 RGB value
                            pLedStrip->Fill(firstLed, numLeds, msg[1], msg[2], msg[3]);
                        else if (paramsLen == 7) // Two RGB values
                            pLedStrip->Fill(firstLed, numLeds, msg[1], msg[2], msg[3], msg[4], msg[5], msg[6]);
                    }
                }
#endif
                break;
            }
            case 0x07: // Control node
            {
#ifdef USE_SPIDEY_GEOM
                printf("Node\r\n");

                if (paramsLen == 11)
                {
                    int nodeIdx = msg[0];
                    if (nodeIdx < sizeof(_spideyNodes)/sizeof(SpideyNodeInfo))
                    {
                        const SpideyNodeInfo& sni = _spideyNodes[nodeIdx];
                        
                        // Handle the node leds
                        printf("Node %d leds %d colour %d,%d,%d ... ", nodeIdx, sni.numNodeLeds, msg[2], msg[3], msg[4]);
                        bool setNodeRGB = ((msg[1] & 0x01) != 0);
                        if (setNodeRGB)
                        {
                            for (int i = 0; (i < sni.numNodeLeds) && (i < MAX_SPIDEY_NODE_LEDS); i++)
                            {
                                pLedStrip->Fill(sni.nodeLeds[i], 1, msg[2], msg[3], msg[4]);
                                printf("Led %d", sni.nodeLeds[i]);
                            }
                        }
                        printf("\r\n");
                        
                        // Handle the link leds
                        int linkIdx = msg[5];
                        int linkLedStart = msg[6];
                        int linkNumLeds = msg[7];
                        printf("Link %d st %d num %d colour %d,%d,%d ... ", linkIdx, linkLedStart, linkNumLeds, msg[8], msg[9], msg[10]);
                        if ((linkIdx < sni.numLinks) && (linkIdx < MAX_SPIDEY_NODE_LINKS))
                        {
                            const SpideyLinkInfo* pSli = sni.nodeLinks[linkIdx];
                            for (int i = 0; i < linkNumLeds; i++)
                            {
                                int ledIdx = linkLedStart + i;
                                int ledNumA = getLedNumFromSeq(pSli->edgeLedsA, pSli->edgeLengthA, ledIdx);
                                if (ledNumA >= 0)
                                {
                                    pLedStrip->Fill(ledNumA, 1, msg[8], msg[9], msg[10]);
                                    printf("A=%d/", ledNumA);
                                }
                                int ledNumB = getLedNumFromSeq(pSli->edgeLedsB, pSli->edgeLengthB, ledIdx);
                                if (ledNumB >= 0)
                                {
                                    pLedStrip->Fill(ledNumB, 1, msg[8], msg[9], msg[10]);
                                    printf("B=%d, ", ledNumB);
                                }
                            }
                        }
                        else
                        {
                            sprintf(responseStr, "%04x NODE - linkInvalid %d", seqCount, linkIdx);                    
                        }
                        printf("\r\n");
                    }
                    else
                    {
                        sprintf(responseStr, "%04x NODE - nodeInvalid %d", seqCount, nodeIdx);                    
                    }
                }
                else
                {
                    sprintf(responseStr, "%04x NODE - BufLengthMismatch %d", seqCount, paramsLen);                    
                }
#endif
                break;
            }
            default:
            {
                sprintf(responseStr, "%04x CMD %02x UNKNOWN", seqCount, cmdCode);
                break;
            }
        }
        
        // Move to next chunk
        nBytesLeft = nBytesLeft - chunkLen - 2;
        msg += paramsLen;
//        printf("Bytesleft = %d, msg[0] = %d\r\n", nBytesLeft, msg[0]);
   }
   return responseStr;
}