#include <string.h>
#include <math.h>
#include <stdio.h>
#include <avr/io.h>

/* These are AVR PORTB pins, +8 to convert to Arduino pins */
#define PS2att 2 //yellow
#define PS2cmd 3 //orange
#define PS2dat 4 //brown
#define PS2clk 5 //blue
//vcc red
//gnd black
#define PS2PORT PORTB
#define PS2IN PINB
#define CTRL_CLK 20
#define CTRL_BYTE_DELAY 20

//These are our button constants
#define PSB_SELECT 0x01
#define PSB_L3 0x02
#define PSB_R3 0x04
#define PSB_START 0x08
#define PSB_PAD_UP 0x10
#define PSB_PAD_RIGHT 0x20
#define PSB_PAD_DOWN 0x40
#define PSB_PAD_LEFT 0x80

#define PSB_L2 0x100
#define PSB_R2 0x200
#define PSB_L1 0x400
#define PSB_R1 0x800
#define PSB_GREEN 0x1000
#define PSB_RED 0x2000
#define PSB_BLUE 0x4000
#define PSB_PINK 0x8000

#define SET(x,y) (x|=(1<<y))
#define CLR(x,y) (x&=(~(1<<y)))
#define CHK(x,y) (x & (1<<y))
#define TOG(x,y) (x^=(1<<y))

#include "WProgram.h"
void base(int dir);
void lwri(int dir);
void grip(int dir);
void rwri(int dir);
void shou(int dir);
void elbo(int dir);
void clear();
void calibrate();
void setup();
void loop();
boolean PSButton(unsigned int button);
unsigned char _gamepad_shiftinout (char byte);
void _gamepad_shiftout (char byte);
unsigned char _gamepad_shiftin();
void read_gamepad();
unsigned char get_gamepad_mode();
void config_gampad();
boolean PSButton();
unsigned char PS2data[9];
void read_gamepad();
void config_gampad();
unsigned char get_gamepad_mode();
unsigned char i;

int state = 0;
int latch1Pin = 15;
int a1Pin = 14;
int b1Pin = 16;
int c1Pin = 17;
int d1Pin = 18;
int latch2Pin = 1;
int a2Pin = 0;
int b2Pin = 2;
int c2Pin = 3;
int d2Pin = 4;
int basePos = 1;
int shouPos = 1;
int elboPos = 1;
int lwriPos = 1;
int rwriPos = 1;
int gripPos = 1;
int setdel = 1;


void base(int dir){
  if (basePos == 1){digitalWrite(d1Pin, HIGH);}
  if (basePos == 2){digitalWrite(a1Pin, HIGH); digitalWrite(d1Pin, HIGH);}
  if (basePos == 3){digitalWrite(b1Pin, HIGH); digitalWrite(d1Pin, HIGH);}
  if (basePos == 4){digitalWrite(a1Pin, HIGH); digitalWrite(b1Pin, HIGH); digitalWrite(d1Pin, HIGH);}
  
  if (dir == 0) basePos++;
  if (dir == 1) basePos--;
  if (basePos == 5) basePos = 1;
  if (basePos == 0) basePos = 4;
}

void lwri(int dir){
  if (lwriPos == 1);
  if (lwriPos == 3){digitalWrite(a1Pin, HIGH);}
  if (lwriPos == 2){digitalWrite(b1Pin, HIGH);}
  if (lwriPos == 4){digitalWrite(a1Pin, HIGH); digitalWrite(b1Pin, HIGH);}
  
  if (dir == 0) lwriPos++;
  if (dir == 1) lwriPos--;
  if (lwriPos == 5) lwriPos = 1;
  if (lwriPos == 0) lwriPos = 4;
}

void grip(int dir){
  if (gripPos == 5) gripPos = 1;
  if (gripPos == 0) gripPos = 4;
  if (gripPos == 1){digitalWrite(c1Pin, HIGH);}
  if (gripPos == 2){digitalWrite(a1Pin, HIGH); digitalWrite(c1Pin, HIGH);}
  if (gripPos == 3){digitalWrite(b1Pin, HIGH); digitalWrite(c1Pin, HIGH);}
  if (gripPos == 4){digitalWrite(a1Pin, HIGH); digitalWrite(b1Pin, HIGH); digitalWrite(c1Pin, HIGH);}

  if (dir == 0) gripPos++;
  if (dir == 1) gripPos--;

}

void rwri(int dir){
  if (rwriPos == 1){}
  if (rwriPos == 2){digitalWrite(b2Pin, HIGH);}
  if (rwriPos == 3){digitalWrite(a2Pin, HIGH);}
  if (rwriPos == 4){digitalWrite(a2Pin, HIGH); digitalWrite(b2Pin, HIGH); }
  
  if (dir == 0) rwriPos++;
  if (dir == 1) rwriPos--;
  if (rwriPos == 5) rwriPos = 1;
  if (rwriPos == 0) rwriPos = 4;
}

void shou(int dir){
  if (shouPos == 1){digitalWrite(c2Pin, HIGH);}
  if (shouPos == 2){digitalWrite(a2Pin, HIGH); digitalWrite(c2Pin, HIGH);}
  if (shouPos == 3){digitalWrite(b2Pin, HIGH); digitalWrite(c2Pin, HIGH);}
  if (shouPos == 4){digitalWrite(a2Pin, HIGH); digitalWrite(b2Pin, HIGH); digitalWrite(c2Pin, HIGH);}
  
  if (dir == 0) shouPos++;
  if (dir == 1) shouPos--;
  if (shouPos == 5) shouPos = 1;
  if (shouPos == 0) shouPos = 4;
}

void elbo(int dir){
  if (elboPos == 1){digitalWrite(d2Pin, HIGH);}
  if (elboPos == 2){digitalWrite(a2Pin, HIGH); digitalWrite(d2Pin, HIGH);}
  if (elboPos == 3){digitalWrite(b2Pin, HIGH); digitalWrite(d2Pin, HIGH);}
  if (elboPos == 4){digitalWrite(a2Pin, HIGH); digitalWrite(b2Pin, HIGH); digitalWrite(d2Pin, HIGH);}
  if (dir == 0) elboPos++;
  if (dir == 1) elboPos--;
  if (elboPos == 5) elboPos = 1;
  if (elboPos == 0) elboPos = 4;
}



void clear(){
  digitalWrite(a1Pin, LOW);
  digitalWrite(b1Pin, LOW);
  digitalWrite(c1Pin, LOW);
  digitalWrite(d1Pin, LOW);
  digitalWrite(a2Pin, LOW);
  digitalWrite(b2Pin, LOW);
  digitalWrite(c2Pin, LOW);
  digitalWrite(d2Pin, LOW);
}

void calibrate(){
    for (int j = 0; j < 4; j++){
    clear();
    base(0);
    digitalWrite(latch1Pin, HIGH);
    digitalWrite(latch2Pin, HIGH);
    digitalWrite(latch1Pin, LOW);
    digitalWrite(latch2Pin, LOW);
    delay(5);
    }
    
    for (int j = 0; j < 4; j++){
    clear();
    shou(1);
    digitalWrite(latch1Pin, HIGH);
    digitalWrite(latch2Pin, HIGH);
    digitalWrite(latch1Pin, LOW);
    digitalWrite(latch2Pin, LOW);    
    delay(5);
    }
    
    for (int j = 0; j < 4; j++){
    clear();
    elbo(1);
    digitalWrite(latch1Pin, HIGH);
    digitalWrite(latch2Pin, HIGH);
    digitalWrite(latch1Pin, LOW);
    digitalWrite(latch2Pin, LOW);
    delay(5);
    }
    
    for (int j = 0; j < 4; j++){
    clear();
    lwri(1);
    digitalWrite(latch1Pin, HIGH);
    digitalWrite(latch2Pin, HIGH);
    digitalWrite(latch1Pin, LOW);
    digitalWrite(latch2Pin, LOW);
    delay(5);
    }
    
    for (int j = 0; j < 4; j++){
    clear();
    rwri(1);
    digitalWrite(latch1Pin, HIGH);
    digitalWrite(latch2Pin, HIGH);
    digitalWrite(latch1Pin, LOW);
    digitalWrite(latch2Pin, LOW);
    delay(5);
    }
    
    for (int j = 0; j < 4; j++){
    clear();
    grip(1);
    digitalWrite(latch1Pin, HIGH);
    digitalWrite(latch2Pin, HIGH);
    digitalWrite(latch1Pin, LOW);
    digitalWrite(latch2Pin, LOW);
    delay(5);
    }
    
}
void setup() {
  pinMode(latch1Pin, OUTPUT);
  pinMode(a1Pin, OUTPUT);
  pinMode(b1Pin, OUTPUT);
  pinMode(c1Pin, OUTPUT);
  pinMode(d1Pin, OUTPUT);
  pinMode(latch2Pin, OUTPUT);
  pinMode(a2Pin, OUTPUT);
  pinMode(b2Pin, OUTPUT);
  pinMode(c2Pin, OUTPUT);
  pinMode(d2Pin, OUTPUT);
  randomSeed(analogRead(0));
  pinMode(PS2clk+8,OUTPUT);
  pinMode(PS2att+8,OUTPUT);
  pinMode(PS2cmd+8,OUTPUT);
  pinMode(PS2dat+8,INPUT);
  digitalWrite(PS2dat+8,HIGH);
  config_gampad();
  digitalWrite(latch1Pin, LOW);
  digitalWrite(latch2Pin, LOW);
  calibrate();
  clear();
}

void loop() {
    read_gamepad();
    clear();
    if(PSButton(PSB_SELECT))
    {while(PSButton(PSB_SELECT)){read_gamepad();}
     setdel+=-1;}
    if(PSButton(PSB_START))
    {while(PSButton(PSB_START)){read_gamepad();}    
      setdel+=1;}
    if(PSButton(PSB_PAD_UP))
     elbo(0);
    if(PSButton(PSB_PAD_RIGHT))
     grip(0);
    if(PSButton(PSB_PAD_LEFT))
     grip(1);
    if(PSButton(PSB_PAD_DOWN))
     elbo(1);
    if(PSButton(PSB_L1))
     lwri(0);
    if(PSButton(PSB_R1))
     rwri(0);
    if(PSButton(PSB_L2))
     lwri(1);
    if(PSButton(PSB_R2))
     rwri(1);
    if(PSButton(PSB_GREEN))
     shou(0);
    if(PSButton(PSB_RED))
     {shou(0); base(0);}
    if(PSButton(PSB_PINK))
     shou(1);
    if(PSButton(PSB_BLUE))
     {shou(1); base(1);}
    digitalWrite(latch1Pin, LOW);
    digitalWrite(latch2Pin, LOW);
    digitalWrite(latch1Pin, HIGH);
    digitalWrite(latch2Pin, HIGH);
    if (setdel < 0) setdel = 0;
    delay(setdel); 
    
  }

boolean PSButton(unsigned int button) {
int byte = 3;
if (button >= 0x100) {
  byte = 4;
  button = button >> 8;
}
  if (~PS2data[byte] & button)
  return true;
  else
  return false;
}
unsigned char _gamepad_shiftinout (char byte) {
   unsigned char tmp = 0;
   for(i=0;i<8;i++) {
	if(CHK(byte,i)) SET(PS2PORT,PS2cmd);
	  else  CLR(PS2PORT,PS2cmd);
	CLR(PS2PORT,PS2clk);
	delayMicroseconds(CTRL_CLK);
	  if(CHK(PS2IN,PS2dat)) SET(tmp,i);
	SET(PS2PORT,PS2clk);
   }
   SET(PS2PORT,PS2cmd);
   delayMicroseconds(CTRL_BYTE_DELAY);
   return tmp;
}
void _gamepad_shiftout (char byte) {
   for(i=0;i<8;i++) {
	if(CHK(byte,i)) SET(PS2PORT,PS2cmd);
	  else  CLR(PS2PORT,PS2cmd);
	CLR(PS2PORT,PS2clk);
	delayMicroseconds(CTRL_CLK);
	SET(PS2PORT,PS2clk);
	//delayMicroseconds(CTRL_CLK);
   }
   SET(PS2PORT,PS2cmd);
   delayMicroseconds(CTRL_BYTE_DELAY);
}
unsigned char _gamepad_shiftin() {
   unsigned char tmp = 0;
   for(i=0;i<8;i++) {
	CLR(PS2PORT,PS2cmd);
	CLR(PS2PORT,PS2clk);
	delayMicroseconds(CTRL_CLK);
	  if(CHK(PS2IN,PS2dat)) SET(tmp,i);
	SET(PS2PORT,PS2clk);
	delayMicroseconds(CTRL_CLK);
   }
   SET(PS2PORT,PS2cmd);
   delayMicroseconds(CTRL_BYTE_DELAY);
   return tmp;
}
void read_gamepad() {
    SET(PS2PORT,PS2cmd);
    SET(PS2PORT,PS2clk);
    CLR(PS2PORT,PS2att); // low enable joystick
    delayMicroseconds(CTRL_BYTE_DELAY);
    char dword[9] = {0x01,0x42,0,0,0,0,0,0,0};
    for (int i = 0; i<9; i++) {
	  PS2data[i] = _gamepad_shiftinout(dword[i]);
    }
    SET(PS2PORT,PS2att); // HI disable joystick
}
unsigned char get_gamepad_mode() {
    SET(PS2PORT,PS2cmd);
    SET(PS2PORT,PS2clk);
    CLR(PS2PORT,PS2att); // low enable joystick
    _gamepad_shiftout(0x01);
    unsigned char x = _gamepad_shiftin();
    SET(PS2PORT,PS2att); // HI disable joystick
    return x;
}
void config_gampad() {
   SET(PS2PORT,PS2cmd);
   SET(PS2PORT,PS2clk);
   CLR(PS2PORT,PS2att); // low enable joystick
    _gamepad_shiftout(0x01);
    _gamepad_shiftout(0x43);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x01);
    _gamepad_shiftout(0x00);
    // Lock to Analog Mode on Stick
    _gamepad_shiftout(0x01);
    _gamepad_shiftout(0x44);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x01);
    _gamepad_shiftout(0x03);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x00);
    // Vibration
    /*
    _gamepad_shiftout(0x01);
    _gamepad_shiftout(0x4D);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x01);
    */
    _gamepad_shiftout(0x01);
    _gamepad_shiftout(0x4F);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0xFF);
    _gamepad_shiftout(0xFF);
    _gamepad_shiftout(0x03);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x01);
    _gamepad_shiftout(0x43);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x00);
    _gamepad_shiftout(0x5A);
    _gamepad_shiftout(0x5A);
    _gamepad_shiftout(0x5A);
    _gamepad_shiftout(0x5A);
    _gamepad_shiftout(0x5A);
   SET(PS2PORT,PS2att);
} 

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}