//*********************************************************
//  *****************************************************
//  * Brian Pescatore, bhp7 & Tom Gowing,teg25          *
//  * Code to read input from Nintendo Entertainment    *
//  * System controllers. The controller uses a         *
//  * serialized mechanism for the data polling. This   *
//  * implementation is configured for two controllers. *
//  *****************************************************
//*********************************************************

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <util/delay.h>
#include "uart.h"
#include "NESController.h"

FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);

/********* Global Variable Declarations *********/

char curr, prev, display, nes1_L, nes2_L;

void initialize (void);
void switch_led (void);
void nes1_update(void);
void nes2_update(void);

// Timer 0 ISR: decrements all ms counters
ISR (TIMER0_COMPA_vect)
{
    if (update_timer>0) --update_timer;
}

// Inverts the order of the button presses such that the NES 6502 will recieve the buttons in expected order.
char flip(char from)
{
    char to = 0;
    to |= (from & 0x01);from >>= 1; to <<= 1;
    to |= (from & 0x01);from >>= 1; to <<= 1;
    to |= (from & 0x01);from >>= 1; to <<= 1;
    to |= (from & 0x01);from >>= 1; to <<= 1;
    to |= (from & 0x01);from >>= 1; to <<= 1;
    to |= (from & 0x01);from >>= 1; to <<= 1;
    to |= (from & 0x01);from >>= 1; to <<= 1;
    to |= (from & 0x01);
    return to;
}

void SPIInit()
{

    // Make all pins input except the MISO
    DDRB = 0x40;

    // Set up SPI
    SPCR = (1<<SPE);
    SPSR = 1;
}

// Entry-point of NESController.c
int main()
{
    initialize();   // Setup the system
    // Start 'er up!
    while(1) {
        // Check if it is time to read the input
        if (update_timer==0) {
            RESET_UPDATE_TIMER;
            nes1_update();
        //  nes2_update();
            SPDR = nes1_L;
        //  PORTD = (~nes1_L );
        }
//      if (SPSR & (1<<SPIF)) {
//          SPDR = nes2_L;
    }
}

// Setup of all data directions and timers
void initialize(void)
{

//  uart_init(); stdout = &uart_str;
//  _delay_us(100);
//  fprintf(stdout,"Starting...\n\r");

    // Clock and latch as output
    NES1_LATCH_DDR |= NES1_LATCH_BIT;       NES2_LATCH_DDR |= NES2_LATCH_BIT;
    NES1_CLOCK_DDR |= NES1_CLOCK_BIT;       NES2_CLOCK_DDR |= NES2_CLOCK_BIT;

    // Data as input
    NES1_DATA_DDR &= ~(NES1_DATA_BIT);      NES2_DATA_DDR &= ~(NES2_DATA_BIT);

    // Enable pullup to avoid errors when no controllers are there.
    NES1_DATA_PORT |= NES1_DATA_BIT;        NES2_DATA_PORT |= NES2_DATA_BIT;

    // Clock is normally high
    NES1_CLOCK_PORT |= NES1_CLOCK_BIT;      NES2_CLOCK_PORT |= NES2_CLOCK_BIT;

    // LATCH is Active HIGH
    NES1_LATCH_PORT &= ~(NES1_LATCH_BIT);   NES2_LATCH_PORT &= ~(NES2_LATCH_BIT);

    curr = prev = 0xFF;
    nes1_L = nes2_L = 0xFF;

    // Set up timer0 for 1 ms
    TIMSK0 = (1 << OCIE0A); // Turn on timer 0 cmp match ISR
    OCR0A  = 249;           // Set the compare reg to 250 time ticks
    TCCR0A = (1 << WGM01);  // Turn on clear-on-match
    TCCR0B = 3;

    SPIInit();

    // Let 'er rip
    sei();
}

// Get the current state of controller 1
void nes1_update(void)
{
    int i; unsigned char tmp=0;

    // Latch pulse to controller
    NES1_LATCH_HIGH(); _delay_us(12); NES1_LATCH_LOW();

    // Loop and shift to read in each button
    for (i=0; i<8; i++) {
        _delay_us(6); NES1_CLOCK_LOW();
        tmp <<= 1;
        if (!NES1_GET_DATA()) { tmp |= 1; }
        _delay_us(6); NES1_CLOCK_HIGH();
    }
    nes1_L = flip(tmp);
}

// Get the current state of controller 2
void nes2_update(void)
{
    int i; unsigned char tmp=0;

    // Latch pulse to controller
    NES2_LATCH_HIGH(); _delay_us(12); NES2_LATCH_LOW();

    // Loop and shift to read in each button
    for (i=0; i<8; i++) {
        _delay_us(6); NES2_CLOCK_LOW();
        tmp <<= 1;
        if (!NES2_GET_DATA()) { tmp |= 1; }
        _delay_us(6); NES2_CLOCK_HIGH();
    }
    nes2_L = flip(tmp);
}