/*
 Arduino NES Controller Input Class
 v1.0
 Toby Deshane <tobyd@hacsoft.org>
 http://hacsoft.org/files/arduino/nes.txt
 2009-09-25, 2:46am
 
 ------------------------
 PIN  FUNCTION    COLOR*
 ------------------------
 1    GND         Brown
 2    CLK         Red 
 3    LATCH       Orange
 4    DATA OUT    Yellow
 5    n/c         --
 6    n/c         --
 7    +5          White
 ------------------------
 
 * color is only guarenteed on official Nintendo products,
 third-party may vary.
 
 (Documentation transcribed from "Game Programming for the
 Propeller-Powered HYDRA".)
 
 NOTES:
 The HYDRA documentation says it's good to use 200ns
 delays, but with the Arduino language, the smallest delay
 is a microsecond (1000ns).  While this may work fine for 
 most, if you're looking to bum some cycles you can replace 
 the delays with assembler NOP opcodes.
 
 Given a 16mhz Arduino, one NOP takes 62.5ns, so four
 of them in a row will give us 250ns.  Close enough, 
 especially considering the act of calling digitalWrite()
 takes some time, too.
 
 [Idea nicked from:
http://msp430rf2500.blogspot.com/2009/08/sub-microsecond-delays-with-arduino.html]
 
 Simply define NES_HIRES to use this technique. 
*/

/*=============================================================*/
#define NES_PIN_LATCH  3
#define NES_PIN_CLOCK  4
#define NES_PIN_DATA   5

// #define NES_HIRES    1

#ifdef NES_HIRES
    #define NES_DELAY
__asm__("nop\n\t""nop\n\t""nop\n\t""nop\n\t""nop\n\t");
#else
    #define NES_DELAY delayMicroseconds(1);
#endif

#define NES_A        (1<<0)
#define NES_B        (1<<1)
#define NES_SELECT   (1<<2)
#define NES_START    (1<<3)
#define NES_UP       (1<<4)
#define NES_DOWN     (1<<5)
#define NES_LEFT     (1<<6)
#define NES_RIGHT    (1<<7)

class NES 
{
private:
    byte latch, clock, data;
public:
    byte button_status;

    void init(byte pin_latch, byte pin_clock, byte pin_data)
    {
        latch = pin_latch;
        clock = pin_clock;
        data = pin_data;

        pinMode(latch, OUTPUT);
        pinMode(clock, OUTPUT);
        pinMode(data, INPUT);

        // Known states
        digitalWrite(latch, LOW);
        digitalWrite(clock, LOW);
    }

    byte poll()
    {
        button_status = 0;

        digitalWrite(latch, HIGH);
        NES_DELAY;
        digitalWrite(latch, LOW);
        if (!digitalRead(data)) button_status |= NES_A;

        digitalWrite(clock, HIGH);
        NES_DELAY;
        digitalWrite(clock, LOW);
        if (!digitalRead(data)) button_status |= NES_B;

        digitalWrite(clock, HIGH);
        NES_DELAY;
        digitalWrite(clock, LOW);
        if (!digitalRead(data)) button_status |= NES_SELECT;

        digitalWrite(clock, HIGH);
        NES_DELAY;
        digitalWrite(clock, LOW);
        if (!digitalRead(data)) button_status |= NES_START;

        digitalWrite(clock, HIGH);
        NES_DELAY;
        digitalWrite(clock, LOW);
        if (!digitalRead(data)) button_status |= NES_UP;

        digitalWrite(clock, HIGH);
        NES_DELAY;
        digitalWrite(clock, LOW);
        if (!digitalRead(data)) button_status |= NES_DOWN;

        digitalWrite(clock, HIGH);
        NES_DELAY;
        digitalWrite(clock, LOW);
        if (!digitalRead(data)) button_status |= NES_LEFT;

        digitalWrite(clock, HIGH);
        NES_DELAY;
        digitalWrite(clock, LOW);
        if (!digitalRead(data)) button_status |= NES_RIGHT;

        return button_status;
    }
};

/*=============================================================*/

NES pad;

void setup()
{
    //Serial.begin(9600);
    pad.init(NES_PIN_LATCH, NES_PIN_CLOCK, NES_PIN_DATA);
}

void loop()
{
    byte buttons = pad.poll();
    
    //if (buttons & NES_START) Serial.println("LET'S A GO!");
}