/*
 * joypad_receiver
 * A wireless Arduino based receiver for ROBOT Controller
 * by Reefat (http://robotics.reefat.com/2009/01/15/arduino-wireless-joypad/)
 * January 15, 2009
 */

#include <stdio.h>
 
int r_sync = 0xAA;    // synchro signal
int r_addr = 0x44;    // receiver address

char buffer[36];

//-------------------------------------------------------------------
// multiple bytes manipulation
//-------------------------------------------------------------------
int getHiByte(int intData) {
    return (intData >> 8);
}

int getLoByte(int intData) {
    return (intData & 0xFF);
}

int mergeHiLo(int hi, int lo) {
    return ( (hi << 8) | lo );
}
//-------------------------------------------------------------------


//-------------------------------------------------------------------
// Each data packet has 4 bytes: | sync | address | data | checksum |
//-------------------------------------------------------------------
class WirelessDataPacket {
    public:
        int address;
        int leftStickX;
        int leftStickY;
        int rightStickX;
        int rightStickY;
        int button;
        int checksum;
        
        WirelessDataPacket(int addr) {
            address = addr;
        }
        
        int sendPacket() {            
            checksum = address + leftStickX + leftStickY + rightStickX + rightStickY + button;
            
            Serial.print(r_sync, BYTE);                    // send sync byte
            Serial.print(address, BYTE);                   // send address byte
            Serial.print(getLoByte(leftStickX), BYTE);     // send leftStickX Low Byte
            Serial.print(getHiByte(leftStickX), BYTE);     // send leftStickX High Byte
            Serial.print(getLoByte(leftStickY), BYTE);     // send leftStickY Low Byte
            Serial.print(getHiByte(leftStickY), BYTE);     // send leftStickY High Byte
            Serial.print(getLoByte(rightStickX), BYTE);    // send rightStickX Low Byte
            Serial.print(getHiByte(rightStickX), BYTE);    // send rightStickX High Byte
            Serial.print(getLoByte(rightStickY), BYTE);    // send rightStickY Low Byte
            Serial.print(getHiByte(rightStickY), BYTE);    // send rightStickY High Byte
            Serial.print(button, BYTE);                    // send button byte
            Serial.print(getLoByte(checksum), BYTE);       // send checksum byte
            Serial.print(getHiByte(checksum), BYTE);       // send checksum byte
            
            return 0;
        }
        
        // wait until a new byte (8 bit) arrives, then return it
        int receiveByte() {
            while(Serial.available() == 0);
            return Serial.read();
        }

        // wait until a new byte pair (16 bit) arrives, then return it
        int receiveBytePair() {
            int loByte, hiByte;
            loByte = receiveByte();
            hiByte = receiveByte();
            return mergeHiLo(hiByte, loByte);
        }
        
        // loop until the byte matches the sync byte
        int findPacketHead() {
            while(1) {
                while(Serial.available() == 0);
                if(Serial.read()==r_sync) {
                    break;
                }
            }
        }
        
        int receivePacket() {
            int addr;
            
            findPacketHead();
            
            addr = receiveByte();
            leftStickX = receiveBytePair();
            leftStickY = receiveBytePair();
            rightStickX = receiveBytePair();
            rightStickY = receiveBytePair();
            button = receiveByte();
            checksum = receiveBytePair();
    
            if(checksum == (addr + leftStickX + leftStickY + rightStickX + rightStickY + button)) {  // checksum verification
                if(addr==address) {  // if receiver address matches
                    sprintf(buffer, "L(%04d, %04d) R(%04d, %04d) B(%04d)", leftStickX, leftStickY, rightStickX, rightStickY, button);
                    Serial.println(buffer);
                    delay(300);
                    Serial.flush();
                }
            } else {
                Serial.println("Error!");
            }
        }
};
WirelessDataPacket WDP(r_addr);
//-------------------------------------------------------------------

void setup() {
    Serial.begin(2400);	// opens serial port, sets data rate to 9600 bps
    pinMode(13, OUTPUT);
}

void loop() {
    WDP.receivePacket();
}