/*
 * joypad_transmitter
 * A wireless Arduino based transmitter 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];

// pin configuration between arduino and joypad
int pinLeftStickX  = 1;    // Analog Pin 1
int pinLeftStickY  = 2;    // Analog Pin 2
int pinRightStickX = 3;    // Analog Pin 3
int pinRightStickY = 4;    // Analog Pin 4

int pinButtonL1 = 9;       // L1 Button
int pinButtonL2 = 10;      // L2 Button
int pinButtonR1 = 11;      // R1 Button
int pinButtonR2 = 12;      // R2 Button

int pinButtonSelect = 8;   // select button
int pinButtonStart = 5;    // start button
int pinButtonMacro = 7;    // macro button
int pinButtonMode = 6;     // mode button

int pinLEDRed = 2;         // red LED pin
int pinLEDYellow = 4;      // yellow LED pin

int releasedButtonMacro = true;    // either the macro button is pressed(false) or released(true)
int releasedButtonMode = true;     // either the mode button is pressed(false) or released(true)

// status
boolean macro = false;
boolean mode  = false;

//-------------------------------------------------------------------
// 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);
        }
        
        int receivePacket() {
            int addr;
            
            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();
                }
            }    
        }
};
WirelessDataPacket WDP(r_addr);
//-------------------------------------------------------------------

int getButtonValue() {
    int value = 0;
    if(digitalRead(pinButtonL1)==LOW) {
        value = value | (1<<7);
    }
    if(digitalRead(pinButtonL2)==LOW) {
        value = value | (1<<6);
    }
    if(digitalRead(pinButtonR1)==LOW) {
        value = value | (1<<5);
    }
    if(digitalRead(pinButtonR2)==LOW) {
        value = value | (1<<4);
    }
    if(digitalRead(pinButtonSelect)==LOW) {
        value = value | (1<<3);
    }
    if(digitalRead(pinButtonStart)==LOW) {
        value = value | (1<<2);
    }
    if(digitalRead(pinButtonMacro)==LOW) {
        if(releasedButtonMacro) {                // if previously released
            macro = ~macro;                      // toggle the macro
            digitalWrite(pinLEDRed, ~macro);     // toggle the red LED
        }
        if(macro) {
            value = value | (1<<1);
        }
        releasedButtonMacro = false;
    } else {
        releasedButtonMacro = true;
    }
    if(digitalRead(pinButtonMode)==LOW) {
        if(releasedButtonMode) {                 // if previously released
            mode = ~mode;                        // toggle the mode
            digitalWrite(pinLEDYellow, ~mode);   // toggle the yellow LED
        }
        if(mode) {
            value = value | (1<<0);
        }
        releasedButtonMode = false;
    } else {
        releasedButtonMode = true;
    }
    
    return value;
}

void setup() {
    pinMode(pinButtonL1, INPUT); digitalWrite(pinButtonL1, HIGH);
    pinMode(pinButtonL2, INPUT); digitalWrite(pinButtonL2, HIGH);
    pinMode(pinButtonR1, INPUT); digitalWrite(pinButtonR1, HIGH);
    pinMode(pinButtonR2, INPUT); digitalWrite(pinButtonR2, HIGH);
    pinMode(pinButtonSelect, INPUT); digitalWrite(pinButtonSelect, HIGH);
    pinMode(pinButtonStart, INPUT); digitalWrite(pinButtonStart, HIGH);
    pinMode(pinButtonMacro, INPUT); digitalWrite(pinButtonMacro, HIGH);
    pinMode(pinButtonMode, INPUT); digitalWrite(pinButtonMode, HIGH);
    pinMode(pinLEDRed, OUTPUT); digitalWrite(pinLEDRed, HIGH);
    pinMode(pinLEDYellow, OUTPUT); digitalWrite(pinLEDYellow, HIGH);
    Serial.begin(2400);	// opens serial port, sets data rate to 9600 bps
}

void loop() {
    WDP.leftStickX  = analogRead(pinLeftStickX);
    WDP.leftStickY  = analogRead(pinLeftStickY);
    WDP.rightStickX = analogRead(pinRightStickX);
    WDP.rightStickY = analogRead(pinRightStickY);
    WDP.button      = getButtonValue();
    WDP.sendPacket();
}