#include "PS2X_lib.h"
#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <avr/io.h>
#if ARDUINO > 22
#include "Arduino.h"
#else
#include "WProgram.h"
#include "pins_arduino.h"
#endif



static byte enter_config[]={0x01,0x43,0x00,0x01,0x00};
static byte set_mode[]={0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00};
static byte set_bytes_large[]={0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00};
static byte exit_config[]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A};
static byte enable_rumble[]={0x01,0x4D,0x00,0x00,0x01};
static byte type_read[]={0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A};

boolean PS2X::NewButtonState() {
    return ((last_buttons ^ buttons) > 0);

}

boolean PS2X::NewButtonState(unsigned int button) {
    return (((last_buttons ^ buttons) & button) > 0);
}

boolean PS2X::ButtonPressed(unsigned int button) {
    return(NewButtonState(button) & Button(button));
}

boolean PS2X::ButtonReleased(unsigned int button) {
    return((NewButtonState(button)) & ((~last_buttons & button) > 0));
}

boolean PS2X::Button(uint16_t button) {
    return ((~buttons & button) > 0);
}

unsigned int PS2X::ButtonDataByte() {
    return (~buttons);
}

byte PS2X::Analog(byte button) {
    return PS2data[button];
}
unsigned char PS2X::_gamepad_shiftinout (char byte) {


    unsigned char tmp = 0;
    for(i=0;i<8;i++) {

        if(CHK(byte,i)) CMD_SET();
        else  CMD_CLR();
        CLK_CLR();

        delayMicroseconds(CTRL_CLK);

        if(DAT_CHK()) SET(tmp,i);
        CLK_SET();
#if CTRL_CLK_HIGH
        delayMicroseconds(CTRL_CLK_HIGH);
#endif	  
    }
    CMD_SET();
    delayMicroseconds(CTRL_BYTE_DELAY);
    return tmp;
}

void PS2X::read_gamepad() {
    read_gamepad(false, 0x00);
}


boolean PS2X::read_gamepad(boolean motor1, byte motor2) {
    double temp = millis() - last_read;

    if (temp > 1500) //waited to long
        reconfig_gamepad();

    if(temp < read_delay)  //waited too short
        delay(read_delay - temp);



    if(motor2 != 0x00)
        motor2 = map(motor2,0,255,0x40,0xFF); //noting below 40 will make it spin

    byte dword[9] = {0x01,0x42,0,motor1,motor2,0,0,0,0};
    byte dword2[12] = {0,0,0,0,0,0,0,0,0,0,0,0};

    // Try a few times to get valid data...
    for (byte RetryCnt = 0; RetryCnt < 5; RetryCnt++) {
        CMD_SET();
        CLK_SET();
        ATT_CLR(); // low enable joystick

        delayMicroseconds(CTRL_BYTE_DELAY);
        //Send the command to send button and joystick data;

        for (int i = 0; i<9; i++) {
            PS2data[i] = _gamepad_shiftinout(dword[i]);
        }


        if(PS2data[1] == 0x79) {  //if controller is in full data return mode, get the rest of data
            for (int i = 0; i<12; i++) {
                PS2data[i+9] = _gamepad_shiftinout(dword2[i]);
            }
        }

        ATT_SET(); // HI disable joystick
        // Check to see if we received valid data or not.  We should be in analog mode for our data
        // to be valie
        if ((PS2data[1] & 0xf0) == 0x70)
            break;

        // If we got to here, we are not in analog mode, try to recover...
        reconfig_gamepad();	// try to get back into Analog mode.
        delay(read_delay);
    }

    // If we get here and still not in analog mode, try increasing the read_delay...
    if ((PS2data[1] & 0xf0) != 0x70) {
        if (read_delay < 10)
            read_delay++;	// see if this helps out...
    }	


#ifdef PS2X_COM_DEBUG
    Serial.println("OUT:IN");
    for(int i=0; i<9; i++){
        Serial.print(dword[i], HEX);
        Serial.print(":");
        Serial.print(PS2data[i], HEX);
        Serial.print(" ");
    }
    for (int i = 0; i<12; i++) {
        Serial.print(dword2[i], HEX);
        Serial.print(":");
        Serial.print(PS2data[i+9], HEX);
        Serial.print(" ");
    }
    Serial.println("");	
#endif

    last_buttons = buttons; //store the previous buttons states

#if defined(__AVR__)
    uint16_t * tmp = (uint16_t*)(PS2data+3);
    buttons = *tmp;
#else
    buttons =  (uint16_t)(PS2data[4] << 8) + PS2data[3];   //store as one value for multiple functions
#endif
    last_read = millis();
    return ((PS2data[1] & 0xf0) == 0x70);
}

byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat) {
    return config_gamepad(clk, cmd, att, dat, false, false);
}


byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat, bool pressures, bool rumble) {

    byte temp[sizeof(type_read)];

#ifdef __AVR__
    _clk_mask = digitalPinToBitMask(clk);
    _clk_oreg = portOutputRegister(digitalPinToPort(clk));
    _cmd_mask = digitalPinToBitMask(cmd);
    _cmd_oreg = portOutputRegister(digitalPinToPort(cmd));
    _att_mask = digitalPinToBitMask(att);
    _att_oreg = portOutputRegister(digitalPinToPort(att));
    _dat_mask = digitalPinToBitMask(dat);
    _dat_ireg = portInputRegister(digitalPinToPort(dat));
#else

    uint32_t            lport;                   // Port number for this pin
    _clk_mask = digitalPinToBitMask(clk); 
    lport = digitalPinToPort(clk);
    _clk_lport_set = portOutputRegister(lport) + 2;
    _clk_lport_clr = portOutputRegister(lport) + 1;

    _cmd_mask = digitalPinToBitMask(cmd); 
    lport = digitalPinToPort(cmd);
    _cmd_lport_set = portOutputRegister(lport) + 2;
    _cmd_lport_clr = portOutputRegister(lport) + 1;

    _att_mask = digitalPinToBitMask(att); 
    lport = digitalPinToPort(att);
    _att_lport_set = portOutputRegister(lport) + 2;
    _att_lport_clr = portOutputRegister(lport) + 1;

    _dat_mask = digitalPinToBitMask(dat); 
    _dat_lport = portInputRegister(digitalPinToPort(dat));

#endif  

    pinMode(clk, OUTPUT); //configure ports
    pinMode(att, OUTPUT);
    pinMode(cmd, OUTPUT);
    pinMode(dat, INPUT);

#if defined(__AVR__)
    digitalWrite(dat, HIGH); //enable pull-up 
#endif

    CMD_SET(); // SET(*_cmd_oreg,_cmd_mask);
    CLK_SET();

    //new error checking. First, read gamepad a few times to see if it's talking
    read_gamepad();
    read_gamepad();

    //see if it talked
    if(PS2data[1] != 0x41 && PS2data[1] != 0x73 && PS2data[1] != 0x79){ //see if mode came back. If still anything but 41, 73 or 79, then it's not talking
#ifdef PS2X_DEBUG
        Serial.println("Controller mode not matched or no controller found");
        Serial.print("Expected 0x41 or 0x73, got ");
        Serial.println(PS2data[1], HEX);
#endif

        return 1; //return error code 1
    }

    //try setting mode, increasing delays if need be. 
    read_delay = 1;

    for(int y = 0; y <= 10; y++)
    {
        sendCommandString(enter_config, sizeof(enter_config)); //start config run

        //read type
        delayMicroseconds(CTRL_BYTE_DELAY);

        CMD_SET();
        CLK_SET();
        ATT_CLR(); // low enable joystick

        delayMicroseconds(CTRL_BYTE_DELAY);

        for (int i = 0; i<9; i++) {
            temp[i] = _gamepad_shiftinout(type_read[i]);
        }

        ATT_SET(); // HI disable joystick

        controller_type = temp[3];

        sendCommandString(set_mode, sizeof(set_mode));
        if(rumble){ sendCommandString(enable_rumble, sizeof(enable_rumble)); en_Rumble = true; }
        if(pressures){ sendCommandString(set_bytes_large, sizeof(set_bytes_large)); en_Pressures = true; }
        sendCommandString(exit_config, sizeof(exit_config));

        read_gamepad();

        if(pressures){
            if(PS2data[1] == 0x79)
                break;
            if(PS2data[1] == 0x73)
                return 3;
        }

        if(PS2data[1] == 0x73)
            break;

        if(y == 10){
#ifdef PS2X_DEBUG
            Serial.println("Controller not accepting commands");
            Serial.print("mode stil set at");
            Serial.println(PS2data[1], HEX);
#endif
            return 2; //exit function with error
        }

        read_delay += 1; //add 1ms to read_delay
    }

    return 0; //no error if here
}



void PS2X::sendCommandString(byte string[], byte len) {


#ifdef PS2X_COM_DEBUG
    byte temp[len];
    ATT_CLR(); // low enable joystick
    delayMicroseconds(CTRL_BYTE_DELAY);

    for (int y=0; y < len; y++)
        temp[y] = _gamepad_shiftinout(string[y]);

    ATT_SET(); //high disable joystick  
    delay(read_delay);                  //wait a few

    Serial.println("OUT:IN Configure");
    for(int i=0; i<len; i++){
        Serial.print(string[i], HEX);
        Serial.print(":");
        Serial.print(temp[i], HEX);
        Serial.print(" ");
    }
    Serial.println("");

#else
    ATT_CLR(); // low enable joystick
    for (int y=0; y < len; y++)
        _gamepad_shiftinout(string[y]);

    ATT_SET(); //high disable joystick  
    delay(read_delay);                  //wait a few
#endif
}



byte PS2X::readType() {
    /*
       byte temp[sizeof(type_read)];

       sendCommandString(enter_config, sizeof(enter_config));

       delayMicroseconds(CTRL_BYTE_DELAY);

       CMD_SET();
       CLK_SET();
       ATT_CLR(); // low enable joystick

       delayMicroseconds(CTRL_BYTE_DELAY);

       for (int i = 0; i<9; i++) {
       temp[i] = _gamepad_shiftinout(type_read[i]);
       }

       sendCommandString(exit_config, sizeof(exit_config));

       if(temp[3] == 0x03)
       return 1;
       else if(temp[3] == 0x01)
       return 2;

       return 0;
     */

    if(controller_type == 0x03)
        return 1;
    else if(controller_type == 0x01)
        return 2;

    return 0;

}

void PS2X::enableRumble() {

    sendCommandString(enter_config, sizeof(enter_config));
    sendCommandString(enable_rumble, sizeof(enable_rumble));
    sendCommandString(exit_config, sizeof(exit_config));
    en_Rumble = true;

}

bool PS2X::enablePressures() {

    sendCommandString(enter_config, sizeof(enter_config));
    sendCommandString(set_bytes_large, sizeof(set_bytes_large));
    sendCommandString(exit_config, sizeof(exit_config));

    read_gamepad();
    read_gamepad();

    if(PS2data[1] != 0x79)
        return false;

    en_Pressures = true;
    return true;
}

void PS2X::reconfig_gamepad(){

    sendCommandString(enter_config, sizeof(enter_config));
    sendCommandString(set_mode, sizeof(set_mode));
    if (en_Rumble)
        sendCommandString(enable_rumble, sizeof(enable_rumble));
    if (en_Pressures)
        sendCommandString(set_bytes_large, sizeof(set_bytes_large));
    sendCommandString(exit_config, sizeof(exit_config));

}


#ifdef __AVR__
inline void  PS2X::CLK_SET(void) {

    register uint8_t old_sreg = SREG;
    cli();
    *_clk_oreg |= _clk_mask;
    SREG = old_sreg;
}

inline void  PS2X::CLK_CLR(void) {
    register uint8_t old_sreg = SREG;
    cli();
    *_clk_oreg &= ~_clk_mask;
    SREG = old_sreg;
}

inline void  PS2X::CMD_SET(void) {
    register uint8_t old_sreg = SREG;
    cli();
    *_cmd_oreg |= _cmd_mask; // SET(*_cmd_oreg,_cmd_mask);
    SREG = old_sreg;
}

inline void  PS2X::CMD_CLR(void) {
    register uint8_t old_sreg = SREG;
    cli();
    *_cmd_oreg &= ~_cmd_mask; // SET(*_cmd_oreg,_cmd_mask);
    SREG = old_sreg;
}

inline void  PS2X::ATT_SET(void) {
    register uint8_t old_sreg = SREG;
    cli();
    *_att_oreg |= _att_mask ; 	
    SREG = old_sreg;
}

inline void PS2X::ATT_CLR(void) {
    register uint8_t old_sreg = SREG;
    cli();
    *_att_oreg &= ~_att_mask; 
    SREG = old_sreg;
}

inline bool PS2X::DAT_CHK(void) {
    return (*_dat_ireg & _dat_mask)? true : false;
}

#else
// On pic32, use the set/clr registers to make them atomic...inline void  PS2X::CLK_SET(void) {
*_clk_lport_set |= _clk_mask;
}

inline void  PS2X::CLK_CLR(void) {
    *_clk_lport_clr |= _clk_mask;
}

inline void  PS2X::CMD_SET(void) {
    *_cmd_lport_set |= _cmd_mask;
}

inline void  PS2X::CMD_CLR(void) {
    *_cmd_lport_clr |= _cmd_mask;
}

inline void  PS2X::ATT_SET(void) {
    *_att_lport_set |= _att_mask;
}

inline void PS2X::ATT_CLR(void) {
    *_att_lport_clr |= _att_mask;
}

inline bool PS2X::DAT_CHK(void) {
    return (*_dat_lport & _dat_mask)? true : false;

}

#endif
