static unsigned char __attribute__ ((progmem)) tiny16_glcd_bmp[]={

0x30, 0xf0, 0xf0, 0xf0, 0xf0, 0x30, 0xf8, 0xbe, 0x9f, 0xff, 0xf8, 0xc0, 0xc0, 0xc0, 0x80, 0x00, 
0x20, 0x3c, 0x3f, 0x3f, 0x1f, 0x19, 0x1f, 0x7b, 0xfb, 0xfe, 0xfe, 0x07, 0x07, 0x07, 0x03, 0x00, };


// cbwp icon ;)
//0x00, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x24, 0x00, 0x00, 0x7e, 0x4a, 0x4a, 0x4a, 0x4a, 0x34, 0x00, 
//0x00, 0x3e, 0x70, 0x18, 0x18, 0x70, 0x3e, 0x00, 0x00, 0x7e, 0x12, 0x12, 0x12, 0x12, 0x0c, 0x00};


static unsigned char __attribute__ ((progmem)) logo16_glcd_bmp[]={
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x80, 0xe0, 0x38, 0xe8, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x80, 0xc0, 0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x40, 0x40, 0x40, 0x40, 
0x40, 0x40, 0x80, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x7c, 
0x24, 0x30, 0x30, 0x38, 0x68, 0x4c, 0x44, 0x44, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x0e, 0x70, 0xc0, 0xf0, 0x10, 0x10, 0x10, 0x10, 0xf0, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x80, 0x60, 0x30, 0x10, 0xf8, 0x08, 0x08, 0x8c, 0x04, 0x04, 0x34, 0x7f, 
0x41, 0x70, 0x1e, 0x07, 0x04, 0x88, 0xf8, 0x18, 0x30, 0x60, 0xc0, 0x00, 0x00, 0xf0, 0x1c, 0x02, 
0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x03, 0x02, 0x0c, 0x16, 0xe2, 0x83, 0x01, 0x01, 0x00, 0x00, 
0x00, 0x00, 0x80, 0x80, 0x40, 0x60, 0x20, 0x20, 0x10, 0x10, 0x18, 0x08, 0x08, 0x04, 0x04, 0x04, 
0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x0c, 
0x08, 0x08, 0x18, 0x30, 0x60, 0xc0, 0x00, 0x00, 0x03, 0x3f, 0xe1, 0x03, 0x02, 0x03, 0x01, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0xf8, 0x0f, 0x01, 0x00, 0x00, 0x00, 0x03, 0x1e, 0xf3, 0x07, 0x06, 0x02, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x01, 0x07, 0xff, 0x00, 0x00, 0x00, 
0x70, 0x70, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x70, 0x30, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x38, 0x38, 0x30, 0x20, 0x00, 0x00, 0x00, 0x0f, 0x78, 0xd8, 0x0c, 0x04, 
0x02, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x06, 0x08, 0x70, 0xcf, 0xf0, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x03, 0x0c, 0x18, 0x70, 0xc0, 0x80, 0x80, 0x00, 0x03, 0x1e, 0x70, 0xc0, 0x00, 0x00, 
0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf8, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x03, 0x02, 0x02, 0x03, 0x0e, 0x3a, 
0xe3, 0x81, 0x01, 0xff, 0x00, 0x00, 0x01, 0x01, 0x03, 0x02, 0x06, 0x0c, 0x0f, 0x70, 0x80, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xfe, 
0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x03, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x0e, 
0x38, 0x60, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfe, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xc0, 0x60, 0x38, 0x0e, 0x03, 
0x07, 0x1c, 0x30, 0xc0, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xc0, 0x60, 0x18, 0x0e, 0x03, 0xff, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x01, 0x02, 0x02, 0x04, 0x04, 0x0c, 0x08, 0x08, 0x18, 0x10, 0x13, 0x7e, 0xd8, 
0x08, 0x0c, 0x04, 0x04, 0x04, 0x02, 0x02, 0x02, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0xe0, 
0x80, 0x00, 0x80, 0xc0, 0x61, 0x12, 0x04, 0x08, 0x10, 0x30, 0x20, 0x20, 0x60, 0x40, 0x40, 0x40, 
0x40, 0xc0, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x40, 
0x60, 0x30, 0x10, 0x08, 0x04, 0x02, 0x01, 0x00, 0x00, 0x00, 0xe0, 0x38, 0x3f, 0x60, 0xb8, 0x08, 
0x08, 0x18, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 
0x03, 0x02, 0x06, 0x04, 0x04, 0x0c, 0x08, 0x08, 0x18, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x18, 
0x0f, 0x0e, 0x07, 0x04, 0x04, 0x06, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x0f, 0x01, 0x00, 0x00, 0x00, 0x01, 0x01, 
0x03, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
};

#include "ST7565.h"
//int ledPin =  13;    // LED connected to digital pin 13
// pin 9 - Serial data out (SID)
// pin 8 - Serial clock out (SCLK)
// pin 7 - Data/Command select (RS or A0)
// pin 6 - LCD reset (RST)
// pin 5 - LCD chip select (CS)
ST7565 glcd(9, 8, 7, 6, 5);

#define WII_IDENT_LEN ((byte)6)
#define WII_TELEGRAM_LEN ((byte)6)
#define WII_NUNCHUCK_TWI_ADR ((byte)0x52)
#include <Wire.h>
#include <string.h>
#include <utility/twi.h>
#include <Servo.h> 
Servo myservo1;  // create servo object to control a servo 
Servo myservo2;  // create servo object to control a servo 
#undef int
#include <stdio.h>
uint8_t outbuf[WII_TELEGRAM_LEN]; // array to store arduino output
int cnt = 0;
int servoPin1 = 3;
int servoPin2 = 4;
int locCx =  90; //moves to location when c button pressed
int locCy =   0;
int locZx = 179; //moves to location when z button pressed
int locZy =  0;
int ledPin = 17;
int servoPos1 = 90;
int servoPos2 = 90;
int pulseWidth = 0;
int pulseWidth2 = 0;
long lastPulse = 0;
long lastPulse2 = 0;
int refreshTime = 20;
int minPulse = 1000;
int minPulse2 = 500;
int dtime=10;
#define pwbuffsize 10
long pwbuff[pwbuffsize];
long pwbuffpos = 0;
long pwbuff2[pwbuffsize];
long pwbuffpos2 = 0;

void setup (){
  Serial.begin (115200);
  Wire.begin(); // initialize i2c
  #define TWI_FREQ_NUNCHUCK 400000L
  TWBR = ((CPU_FREQ / TWI_FREQ_NUNCHUCK) - 16) / 2;
  nunchuck_init(0); // send the initialization handshake
  pinMode(servoPin1, OUTPUT);      
  pinMode(servoPin2, OUTPUT);      
  pinMode(ledPin, OUTPUT);      
  pulseWidth = minPulse;
  pulseWidth2 = minPulse2;
  byte i;
  if(readControllerIdent(outbuf) == 0){
    Serial.print("Ident=");
    for (i = 0; i < WII_TELEGRAM_LEN; i++){
      Serial.print(outbuf[i], HEX);
      Serial.print(' ');
    }
    Serial.println();
  }
  myservo1.attach(servoPin1);  // attaches the servo on pin 6 to the servo object 
  myservo2.attach(servoPin2);  // attaches the servo on pin 7 to the servo object 
  Serial.print(freeRam());
  glcd.begin(0x18);
  glcd.display(); // show splashscreen
  delay(2000);
  glcd.clear();
  glcd.drawbitmap(0, 0, logo16_glcd_bmp, 128, 64, BLACK);
  glcd.display();
  Serial.println("Finished setup");
}

byte nunchuck_init (unsigned short timeout){
  byte rc = 1;
    unsigned long time = millis();
    do{
      Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
      Wire.send (0xF0); // sends memory address
      Wire.send (0x55); // sends data.
      if(Wire.endTransmission() == 0){ // stop transmitting
        Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
        Wire.send (0xFB); // sends memory address
        Wire.send (0x00); // sends sent a zero.
        if(Wire.endTransmission () == 0){ // stop transmitting
          rc = 0;
        }
      }
    }
    while (rc != 0 && (!timeout || ((millis() - time) < timeout)));
  return rc;
}

byte readControllerIdent(byte* pIdent){
  byte rc = 1;
  Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
  Wire.send (0xFA); // sends memory address of ident in controller
  if(Wire.endTransmission () == 0){ // stop transmitting
    byte i;
    Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
    for (i = 0; (i < WII_TELEGRAM_LEN) && Wire.available (); i++){
      pIdent[i] = Wire.receive(); // receive byte as an integer
    }
    if(i == WII_TELEGRAM_LEN){
      rc = 0;
    }
  }
  return rc;
}

void clearTwiInputBuffer(void){
  while( Wire.available ())
  Wire.receive ();
}

void send_zero (){
  for(byte i = 0; i < 3; i++){
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.send (0x00); // sends one byte
    Wire.endTransmission (); // stop transmitting
  }
}

int t = 0;
void loop (){
  t++;
  long last = millis();
//
  if( t == 1) {
    t = 0;
//
  Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
  for (cnt = 0; (cnt < WII_TELEGRAM_LEN) && Wire.available (); cnt++){
    outbuf[cnt] = nunchuk_decode_byte (Wire.receive ()); // receive byte as an integer
  }
  clearTwiInputBuffer();
  if (cnt >= WII_TELEGRAM_LEN){  // If we recieved the 6 bytes, then go print them
    print ();
    int z_button = 0;
    int c_button = 0;
    if ((outbuf[5] >> 0) & 1){ z_button = 1; }
    if ((outbuf[5] >> 1) & 1){ c_button = 1; }
    if (c_button && z_button) {  muovi(); }
  }
  send_zero (); // send the request for next bytes

//
  }
  updateServo();
//
  delay (20);
  int slopeX = random(1,10);
  int slopeY = random(1,10);
  fire(slopeX,slopeY);

}

void print (){
  digitalWrite (ledPin, HIGH); // sets the LED on
  delay(10);
  digitalWrite (ledPin, LOW); // sets the LED off
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];
  int accel_x_axis = outbuf[2];// * 2 * 2
  int accel_y_axis = outbuf[3];// * 2 * 2
  int accel_z_axis = outbuf[4];// * 2 * 2
  int z_button = 0;
  int c_button = 0;

  if ((outbuf[5] >> 0) & 1){ z_button = 1; }
  if ((outbuf[5] >> 1) & 1){ c_button = 1; }

  if (!c_button){
    servoPos1=locCx;                  // sets the servo position according to the scaled value 
    servoPos2=locCy;                  // sets the servo position according to the scaled value 
    myservo1.write(servoPos1);                  // sets the servo position according to the scaled value 
    myservo2.write(servoPos2);                  // sets the servo position according to the scaled value 
    delay(500);                           // waits for the servo to get there 
  }
  if (!z_button){
    servoPos1=locZx;                  // sets the servo position according to the scaled value 
    servoPos2=locZy;                  // sets the servo position according to the scaled value 
    myservo1.write(servoPos1);                  // sets the servo position according to the scaled value 
    myservo2.write(servoPos2);                  // sets the servo position according to the scaled value 
    delay(500);                           // waits for the servo to get there 
  }

Serial.print (joy_x_axis, DEC);
Serial.print ("\t");
Serial.print (joy_y_axis, DEC);
Serial.print ("\t");
Serial.print (accel_x_axis, DEC);
Serial.print ("\t");
Serial.print (accel_y_axis, DEC);
Serial.print ("\t");
Serial.print (accel_z_axis, DEC);
Serial.print ("\t");
Serial.print (z_button, DEC);
Serial.print ("\t");
Serial.print (c_button, DEC);
Serial.print ("\t");
Serial.print ("\r\n");

  if(
    (joy_x_axis == 255) && 
    (joy_y_axis == 255) && 
    (accel_x_axis == 255) && 
    (accel_y_axis == 255) && 
    (accel_z_axis == 255) && 
    (joy_x_axis == 1) && 
    (joy_x_axis == 1)
  ){ setup(); }
}

char nunchuk_decode_byte (char x){
  return x;
}

void muovi (){
  float serv1 = outbuf[0];// joy x 0-255 
  if (serv1 < 200 && serv1 > 50){
    // centered
  } else if (serv1 > 250){
    servoPos1=servoPos1-5;
  } else if (serv1 > 200){
    servoPos1=servoPos1-2;
  } else if (serv1 < 5){
    servoPos1=servoPos1+5;
  } else if (serv1 < 50){
    servoPos1=servoPos1+2;
  }
    
  float serv2 = outbuf[1];// joy y 0-255
  if (serv2 < 200 && serv2 > 50){
  } else if (serv2 > 250){
    servoPos2=servoPos2+5;
  } else if (serv2 > 200){
    servoPos2=servoPos2+2;
  } else if (serv2 < 5){
    servoPos2=servoPos2-5;
  } else if (serv2 < 50){
    servoPos2=servoPos2-2;
  }
  if (servoPos1 < 0){ servoPos1 = 0; }
  if (servoPos1 > 179){ servoPos1 = 179; }
  if (servoPos2 < 0){ servoPos2 = 0; }
  if (servoPos2 > 179){ servoPos2 = 179; }

Serial.print (servoPos1, DEC);
Serial.print ("\t");
Serial.print (servoPos2, DEC);
Serial.print ("\t");
Serial.print ("\r\n");
}

void updateServo() {
  myservo1.write(servoPos1);                  // sets the servo position according to the scaled value 
  myservo2.write(servoPos2);                  // sets the servo position according to the scaled value 
  delay(250);                           // waits for the servo to get there 
}
void fire(int sx, int sy){
  int x=0;int y=112;//128 px-16
  while(x<64){
    while(y>0){
  delay(20);
  
  glcd.clear();
  glcd.drawbitmap(0, 0, logo16_glcd_bmp, 128, 64, BLACK);
  glcd.drawbitmap(x, y, tiny16_glcd_bmp, 16, 16, BLACK);
  glcd.display();
  x=x+sx;
  y=y-sy;
  //if(x>63){ x=0; }
  if(y<1){ y=112; }
    }
  }
}
int freeRam(void){
  extern int  __bss_end; 
  extern int  *__brkval; 
  int free_memory; 
  if((int)__brkval == 0) {
    free_memory = ((int)&free_memory) - ((int)&__bss_end); 
  }
  else {
    free_memory = ((int)&free_memory) - ((int)__brkval); 
  }
  return free_memory; 
}