#include "ht1632.h"

#define  WIN 0
#define  PONG  1
#define  CLS    2
/*
 * Set these constants to the values of the pins connected to the SureElectronics Module
 */
//static const byte ht1632_data = 6;     // Data pin (pin 7)
//static const byte ht1632_wrclk = 7;    // Write clock pin (pin 5)
//static const byte ht1632_cs[] = {8,9};       // Chip Select (1, 2, 3, or 4)
static const byte ht1632_data = 3;     // Data pin (pin 7)
static const byte ht1632_wrclk = 4;    // Write clock pin (pin 5)
static const byte ht1632_cs[] = {5,6};       // Chip Select (1, 2, 3, or 4)
static const byte ht1632_displays = 2; // Number of displays

static const byte p1control = 0; 
static const byte p2control = 1;
static const byte button = 2;
static const byte speaker = 12;

unsigned long prevmillis = 0;

static const byte bitmaps[][128]={
                    {12,12,9,9,3,3,6,6,12,12,9,9,3,3,6,6,12,12,9,9,3,3,6,6,12,12,9,9,3,3,6,6,12,12,9,9,3,3,6,6,12,12,9,9,3,3,6,6,12,12,9,9,3,3,6,6,12,12,9,9,3,3,6,6,3,3,9,9,12,12,6,6,3,3,9,9,12,12,6,6,3,3,9,9,12,12,6,6,3,3,9,9,12,12,6,6,3,3,9,9,12,12,6,6,3,3,9,9,12,12,6,6,3,3,9,9,12,12,6,6,3,3,9,9,12,12,6,6},
                    {0,0,15,15,15,15,12,12,12,12,15,12,7,8,0,0,7,15,15,15,12,0,12,0,15,15,7,15,0,0,15,15,7,15,3,0,1,8,15,15,15,15,0,0,7,15,15,15,12,0,12,0,14,0,6,0,0,0,15,15,15,15,0,0,0,0,15,15,15,15,0,0,0,0,0,0,0,0,0,0,15,14,15,15,0,3,0,3,15,15,15,14,0,0,15,15,15,15,0,0,0,0,15,15,15,15,0,0,15,14,15,15,0,3,3,3,3,15,3,14,0,0,15,11,15,11,0,0},
                    {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
                };


byte ht1632_shadowram[128];

/*
 * ht1632_writebits
 * Write bits (up to 8) to h1632 on pins ht1632_data, ht1632_wrclk
 * Chip is assumed to already be chip-selected
 * Bits are shifted out from MSB to LSB, with the first bit sent
 * being (bits & firstbit), shifted till firsbit is zero.
 */
void ht1632_chipselect(byte chipno)
{
  digitalWrite(chipno, 0);
}

void ht1632_chipfree(byte chipno)
{
  digitalWrite(chipno, 1);
}

void ht1632_writebits (byte bits, byte firstbit)
{
  while (firstbit) {
    digitalWrite(ht1632_wrclk, LOW);
    if (bits & firstbit) {
      digitalWrite(ht1632_data, HIGH);
    }
    else {
      digitalWrite(ht1632_data, LOW);
    }
    digitalWrite(ht1632_wrclk, HIGH);
    firstbit >>= 1;
  }
}

static void ht1632_sendcmd (byte command,byte cs)
{
  ht1632_chipselect(cs);  // Select chip
  ht1632_writebits(HT1632_ID_CMD, 1<<2);  // send 3 bits of id: COMMMAND
  ht1632_writebits(command, 1<<7);  // send the actual command
  ht1632_writebits(0, 1); 	/* one extra dont-care bit in commands. */
  ht1632_chipfree(cs); //done
}

static void ht1632_senddata (byte address, byte data, byte cs)
{
  ht1632_chipselect(cs);  // Select chip
  ht1632_writebits(HT1632_ID_WR, 1<<2);  // send ID: WRITE to RAM
  ht1632_writebits(address, 1<<6); // Send address
  ht1632_writebits(data, 1<<3); // send 4 bits of data
  ht1632_chipfree(cs); // done
}

static void ht1632_sendshadowram ()
{
  for(byte cs=0;cs<ht1632_displays;cs++){
    ht1632_chipselect(ht1632_cs[cs]);  // Select chip
    ht1632_writebits(HT1632_ID_WR, 1<<2);  // send ID: WRITE to RAM
    ht1632_writebits(0,1<<6); // Send address
    for(byte i=cs*64;i<((cs+1)*64);i++)
      ht1632_writebits(ht1632_shadowram[i], 1<<3); // send 4 bits of data
    ht1632_chipfree(ht1632_cs[cs]); // done
  }
}

void setup ()  // flow chart from page 17 of datasheet
{
//  Serial.begin(115200);          
  pinMode(button,INPUT_PULLUP);
  pinMode(ht1632_wrclk, OUTPUT);
  pinMode(ht1632_data, OUTPUT);

  for(byte j=0;j<ht1632_displays;j++){
    pinMode(ht1632_cs[j], OUTPUT);
    digitalWrite(ht1632_cs[j], HIGH); 	/* unselect (active low) */
    ht1632_sendcmd(HT1632_CMD_SYSDIS,ht1632_cs[j]);  // Disable system
    ht1632_sendcmd(HT1632_CMD_COMS10,ht1632_cs[j]);  // 16*32, PMOS drivers
    ht1632_sendcmd(HT1632_CMD_MSTMD,ht1632_cs[j]); 	/* Master Mode */
    ht1632_sendcmd(HT1632_CMD_SYSON,ht1632_cs[j]); 	/* System on */
    ht1632_sendcmd(HT1632_CMD_LEDON,ht1632_cs[j]); 	/* LEDs on */
    ht1632_sendcmd(HT1632_CMD_PWM | 15 ,ht1632_cs[j]); /* pwm off */
    for (byte i=0; i<64; i++)
      ht1632_senddata(i, 15,ht1632_cs[j]);  // clear the display!
    delay(100);
    for (byte i=0; i<64; i++)
      ht1632_senddata(i, 0,ht1632_cs[j]);  // clear the display!
  }
  randomSeed(analogRead(3));
  
}

void plot (char x, char y, char val)
{
  char addr, bitval, offset;
  
  /*
   * The 4 bits in a single memory word go DOWN, with the LSB
   * (last transmitted) bit being on top.  However, writebits()
   * sends the LSB first, so we have to do a sort of bit-reversal
   * somewhere.  Here, this is done by shifting the single bit in
   * the opposite direction from what you might expect.
   */

//  offset = (y&B11111000)<<3; // compute offset (for y>7)
  offset = y>>3;
  bitval = 8>>(y&3);  // compute which bit will need set
  addr = (x<<1) + ((y&B00000111)>>2) + (offset<<6);  // compute which memory word this is in
  if (val) {  // Modify the shadow memory
    ht1632_shadowram[addr] |= bitval;
  }  else {
    ht1632_shadowram[addr] &= ~bitval;
  }
  // Now copy the new memory value to the display
  //for(byte j=0;j<ht1632_displays;j++){
    ht1632_senddata(addr, ht1632_shadowram[addr],ht1632_cs[offset]);
  //}
}

void showbitmap(char bitmap){
  byte i;
  for(i=0;i<128;i++){
    ht1632_shadowram[i]=bitmaps[bitmap][i];
  }
  ht1632_sendshadowram();
}

void bat(byte x, byte y, byte bs, byte value){
  for(bs; bs>0; bs--){
    plot(x,y+bs-1,value);
  }
}


int game() {
  // x & y scaled by 8 zodat er interger math gedaan kan worden voor de rc
  signed int X=16*8; // ball x 
  signed int Y=8*8;  // ball y 
  signed int Xo=X;   // ball x previous
  signed int Yo=Y;   // ball y previos
  signed int XRC;  // X direction 
  signed int YRC=0;  // y direction
  signed int p1b=300;  // player 1 bat position
  signed int p2b=300; // player 2 bat position
  signed int p1bo=400;  // player 1 bat old position
  signed int p2bo=400; // player 2 bat old position
  unsigned int ballspeed=80; // ball speed
  byte batsize=5;
  byte gamecountdown=1;
  byte countdown=7;

  if(random(2)==1){
    XRC=8;
  }else{
    XRC=-8;
  }
  YRC=random(-4,4);
  
   while(true) {
    // pre game beeping
    if(gamecountdown==1){
      if((millis()-prevmillis)>150){
        prevmillis=millis();
        if(countdown>0){
          if(countdown%2==1){
            plot(16,8,1);
            tone(speaker,1000,10);          
          } else {
            plot(16,8,0);
          }
          countdown--;
        }
      }
      if(countdown==0){
        tone(speaker,1400,20);
//      bat(16,0,16,1); 
        gamecountdown=0;
      }
    } else {

      // ball shizzle
      if ((millis()-prevmillis)>ballspeed){
        prevmillis=millis();
        X+=XRC;
        Y+=YRC;

        if(X>255){
          tone(speaker,100,10);
          return(1);
        }

        // p1 paddle check 
        if((X>>3) == 3) {
          if(((Y>>3)>=(p1b>>3)) & ((Y>>3)<((p1b>>3)+batsize))){
            tone(speaker,400,5);
             XRC=-XRC;
             if(ballspeed>0){
               ballspeed-=5;
             }
             if(YRC>=0){
               YRC=map(Y-p1b,0,batsize<<3,0,16);
             } else {
               YRC=map(Y-p1b,0,batsize<<3,-16,0);
             }
          } 
        }
 
        // p2 paddle check 
        if((X>>3) == 28) {
          if(((Y>>3)>=(p2b>>3)) & ((Y>>3)<((p2b>>3)+batsize))){
            tone(speaker,400,5);
             XRC=-XRC;
             if(ballspeed>0){
               ballspeed-=5;
            }  
             if(YRC>=0){
               YRC=map(Y-p2b,0,batsize<<3,0,16);
             } else {
               YRC=map(Y-p2b,0,batsize<<3,-16,0);
             }
          } 
        }
   
        // p2 scores , reset ball
        if(X<1){
          tone(speaker,100,10);
          return(2);
        }
      
        // bounce onderkant
        if(Y>127) {
          tone(speaker,500,1);
          Y=127;
          YRC=-YRC;
        }
        // bounce bovenkant
        if(Y<0) {
          tone(speaker,500,1);
          Y=0;
          YRC=-YRC;
        }
      
        // plot bal, en verwijder de oude
        plot(X>>3,Y>>3,1);
        if((X>>3!=Xo>>3) | (Y>>3!=Yo>>3)) {
          plot(Xo>>3,Yo>>3,0);
          Xo=X;
          Yo=Y;
        } 
      }
    }
    // player 1 pad
    p1b=map(analogRead(p1control)>>1,0,512,0,128-((batsize-1)<<3));
    if((p1b>>3)!=(p1bo>>3)){
      bat(2,p1bo>>3,batsize,0);
      bat(2,p1b>>3,batsize,1);
      p1bo=p1b;
    }

    // player 2 pad
    p2b=map(analogRead(p2control)>>1,0,512,0,128-((batsize-1)<<3));
//    p2b=0;
    if((p2b>>3)!=(p2bo>>3)){
      bat(29,p2bo>>3,batsize,0);
      bat(29,p2b>>3,batsize,1);
      p2bo=p2b;
    }
  }
}

void p1winanim(byte loops, int dely ){
  for (loops ; loops>0 ; loops--){
    for (byte offset=0;offset<8;offset+=2){
      for (byte i=0;i<64;i++){
        ht1632_shadowram[i]=bitmaps[WIN][(i+offset)&63];
        ht1632_shadowram[i+64]=bitmaps[WIN][64+((i+offset)&63)];
      }
      ht1632_sendshadowram();
      delay(dely);
    }
  }
}

void p2winanim(byte loops, int dely){
  for (loops ; loops>0 ; loops--){
    for (byte offset=8;offset>0;offset-=2){
      for (byte i=0;i<64;i++){
        ht1632_shadowram[i+64]=bitmaps[WIN][(i+offset)&63];
        ht1632_shadowram[i]=bitmaps[WIN][64+((i+offset)&63)];
      }
      ht1632_sendshadowram();
      delay(dely);
    }
  }
}

void loop(){
  byte p1s;
  byte p2s;
  p1s=0;
  p2s=0;

  showbitmap(PONG);  
  while(digitalRead(button)==1){
    delay(10);
  }


  
  while(true){
    showbitmap(CLS);
    bat(0,0,p1s,1);
    bat(0,16-p1s,p1s,1);
    bat(31,0,p2s,1);
    bat(31,16-p2s,p2s,1);
   
    if(game()==1){
      p1s++;
    }else{
      p2s++;
    }
    if(p1s==9){
      p1winanim(10,100);
      break;
    }
    if(p2s==9){
      p2winanim(10,100);
      break;
    }
  }
  }