File content as of revision 19:8c1f2a2204fb:

#include <InterruptIn.h>

#define PACKETSIZE 256

// Example
// Packet 1. (SuperPackid=5,type=PT_IMAGE,size=0)
// Packet 2. (SuperPackid=5,type=PT_DEFAULT,size=1024)
// Packet 3. (SuperPackid=5,type=PT_DEFAULT,size=1000)
// Packet 4. (SuperPackid=5,type=PT_END,size=0)
#define XBEEON
enum PACKET_TYPE{
    PT_EMPTY=0,
    PT_DEFAULT,
    PT_END,
    PT_IMAGE,
    PT_IMAGEHEAD,
    PT_REQLOC,
    PT_SENDLOC,
    PT_WAY,
    PT_GETCOMMANDS,
    PT_SIZE
};
typedef struct PacketStruct{
    unsigned int type;
    unsigned int size;// Number of valid bits
    char data[PACKETSIZE];
    unsigned int superPackID;// 
    char special[4];// Set to FF when
}PacketStruct;

class PacketSender{
    private:
    unsigned int superID;
    public:
    unsigned int getSuperID(){return superID++;}
    PacketSender():superID(0),outputDevice(
        #ifdef XBEEON
        XBEE::getSerial()
        #else
        USB::getSerial()
        #endif
    ),setTCPConStatus(
        XBEE::getTCPConOut()
    ),getTCPConStatus(
        XBEE::getTCPConIn()
    ),next(NULL){
        //outputDevice.attach(this,&PacketSender::handleUpdate,Serial::RxIrq);
        lastValid=NULL;
    }
    Serial& outputDevice;
    DigitalOut& setTCPConStatus;
    DigitalIn& getTCPConStatus;
    void sendPacket(PacketStruct& output){
        for(int a=0;a<sizeof(PacketStruct);a++){
            while(!outputDevice.writeable()){}
            outputDevice.putc(((char*)(&output))[a]);
            //wait_us(10);
            //USB::getSerial().putc(((char*)(&output))[a]);
        }
        //wait_ms(100);
    }
    
    void wait_for_egg(){
        char response[40];
        while(1){
            while(!outputDevice.readable()){}
            outputDevice.scanf("%s",response);
            if(strcmp(response,"DHCP:") == 0){
                USB::getSerial().printf("Egg Connected!\r\n");
                for(int a=0;a<100000000;a++);
                while(outputDevice.readable()){
                    outputDevice.getc();
                }
                return;
            }
        }
    }
    
    void openConnection(char close_conn = 0){
        __disable_irq();
        do{
            USB::getSerial().printf("trying to connect...\r\n");
            if(getTCPConStatus){
                setTCPConStatus = 0;
                while(getTCPConStatus){}
                wait_us(50000);
            }
            setTCPConStatus = 1;
            while(setTCPConStatus==1 && !getTCPConStatus){}          
        }while(!getTCPConStatus);

        __enable_irq();
        //for(int a=0;a<10000000;a++);
//        char connected = 0;
//        char in_cmd_mode = 0;
//        char response[80];
//        int charcount;
//        
//        while(in_cmd_mode == 0){
//            //get into command mode
//            while(!outputDevice.writeable()){}
//            outputDevice.putc('$');
//            while(!outputDevice.writeable()){}
//            outputDevice.putc('$');
//            while(!outputDevice.writeable()){}
//            outputDevice.putc('$');
//            while(!outputDevice.writeable()){}
//            while(!outputDevice.readable()){}
//            //outputDevice.scanf("%s",response);
//            charcount = 0;
//            while(outputDevice.readable()){
//                response[charcount] = outputDevice.getc();
//                charcount++;
//                for(int a=0;a<10000;a++);
//            }
//            response[charcount] = 0;
//            USB::getSerial().printf("%s",response);
//            if(strcmp(response+strlen(response)-5,"CMD\r\n") == 0){
//                in_cmd_mode = 1;
//            }else{
//                while(1);
//            }
//            USB::getSerial().printf("in cmd mode: %d\r\n",in_cmd_mode);
//        }
//        //open the connection!
//        while(connected == 0){
//            for(int a=0;a<10000000;a++);
//            while(!outputDevice.writeable()){}
//            if(close_conn){
//                outputDevice.printf("close");
//                while(!outputDevice.writeable()){}
//                outputDevice.putc(0);
//                while(!outputDevice.writeable()){}
//                outputDevice.printf("exit");
//            }else{
//                outputDevice.printf("open 1.2.3.14 7000");
//            }
//            while(!outputDevice.writeable()){}
//            outputDevice.putc(0);
//            while(!outputDevice.writeable()){}
//            if(0 && !close_conn){
//                for(int a=0;a<5;a++){
//                    while(!outputDevice.readable()){}
//                    outputDevice.scanf("%s",response);
//                    USB::getSerial().printf("%s ",response);
//                }
//            }
//            while(!outputDevice.readable()){}
//            for(int a=0;a<10000000;a++);
//            charcount = 0;
//            while(outputDevice.readable()){
//                response[charcount] = outputDevice.getc();
//                USB::getSerial().putc(response[charcount]);
//                charcount++;
//                for(int a=0;a<10000;a++);
//            }
//            response[charcount] = 0;
//            USB::getSerial().printf("RESPONSE: %s\r\n",response);
//            if(close_conn){
//                connected = 1;
//            }else{
//                if(strcmp(response+strlen(response)-6,"*OPEN*") == 0){
//                    connected = 1;
//                }
//            }
//            USB::getSerial().printf("connected: %d\r\n",connected);
//        }
//        for(int a=0;a<1000000;a++);
    }
    void closeConnection(){
        __disable_irq();
        wait_us(50000);
        do{
            USB::getSerial().printf("disconnecting...\r\n");
            setTCPConStatus = 0;
            wait_us(50000);
        }while(getTCPConStatus);
    }
    unsigned int min(unsigned int a,unsigned int b){
        return a<b ? a : b;
    }
    void sendPacket(unsigned int superPackID,char* data,unsigned int size,PACKET_TYPE type = PT_END){
        if(data!=NULL && size>0){
            for(int i=0;i<=(size-1)/PACKETSIZE;i++){
                PacketStruct output;
                output.type=PT_DEFAULT;
                output.superPackID=superPackID;
                output.size=min(PACKETSIZE,size-i*PACKETSIZE);
                for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
                for(int a=0;a<output.size;a++){output.data[a]=data[a-i*PACKETSIZE];}
                for(int a=output.size;a<PACKETSIZE;a++){output.data[a]='\0';}
                sendPacket(output);
            }
        }else{
            //openConnection();
            PacketStruct output;
            output.type=type;
            output.size=0;
            output.superPackID=superPackID;
            for(int a=0;a<4;a++){output.special[a]='\0';}output.special[3]=0xAA;
            // Check for empty packet
            if(output.type==PT_EMPTY){output.type=0;output.size=0;output.superPackID=0;output.special[3]=0xFF;}
            for(int a=0;a<PACKETSIZE;a++){output.data[a]='\0';}
            sendPacket(output);
        }
        if(type == PT_END){
            //closeConnection();
        }
    }
    PacketStruct* lastValid;
    bool found;
    void handleUpdate(){
        USB::getSerial().printf("Interupt\n");
        PacketStruct* next = getNextPacket();
        if(next!=NULL){lastValid=next;}
    }
    
    // Number of consecutive zeros
    unsigned int numZeros;
    // Return true if a resync command has been received
    bool resetCheck(char input){
        if(input=='\0'){
            numZeros++;
        }else if(numZeros==sizeof(PacketStruct)-1&&input==0xFF){
            return true;
        }else{
            numZeros=0;
        }
        return false;
    }
    
    // Temperary storage for next valid
    PacketStruct* next;
    // Number of valid bits in next packet
    int nextValid;
    /// \brief Grab the next packet
    PacketStruct* getNextPacket(){
        // Check for null packet
        if(next==NULL){next=new PacketStruct();nextValid=0;}
        // Create reset packet which resets on re-sync command
        bool resetPacket=false;

        // While there is data to read
        while(0<outputDevice.readable()){
            // Check if a full packet has been received
            if(nextValid==sizeof(PacketStruct))break;
            // Read in next char
            char input=outputDevice.getc();
            USB::getSerial().printf("Read ByteC %X %X\n",nextValid,input);
            // Check for valid char
            if(resetCheck(input)){resetPacket=true;break;}
            // Set char
            ((char*)next)[nextValid++] = input;
        }
        
        if(nextValid==sizeof(PacketStruct)||resetPacket){
            // Reset packet
            PacketStruct* output=next;next=NULL;
            // Return
            return resetPacket?NULL:output;
        }
        return NULL;
/*        
        int avail = outputDevice.readable();
        if(avail <= 0)return NULL;
        PacketStruct* output=new PacketStruct();
        for(int i=0;i<sizeof(PacketStruct);i++){
            // Wait for byte
            while(outputDevice.readable()<=0){}
            ((char*)output)[i] = outputDevice.getc();
        }
        return output;
*/
    }
    
};
static PacketSender* ps=NULL;
static PacketSender& getPS(){
    if(ps==NULL)ps=new PacketSender();
    return *ps;
}