#include <MD_MAX72xx.h>
#include "U8glib.h"

//Matrix
#define	CLK_PIN		12
#define	DATA_PIN	11
#define	CS_PIN		10

MD_MAX72XX mx = MD_MAX72XX(MD_MAX72XX::GENERIC_HW, DATA_PIN, CLK_PIN, CS_PIN,1);
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST);

int x = 0;
int y = 0;
int beginDraw = 150;

//Ultra 1
#define PIN_TRIG1 3
#define PIN_TRIG2 4
#define PIN_TRIG3 5
#define PIN_ECHO 2

void setup() {

  u8g.setFont(u8g_font_tpssb);
  u8g.setColorIndex(1);

  mx.begin();
  mx.control(MD_MAX72XX::INTENSITY, MAX_INTENSITY / 2);
  mx.clear();
  Serial.begin(115200);
  pinMode(PIN_TRIG3,OUTPUT);
  pinMode(PIN_TRIG2,OUTPUT);
  pinMode(PIN_TRIG1,OUTPUT);
  pinMode(PIN_ECHO, INPUT);

    
}

//mitsu image
const uint8_t rook_bitmap[72] U8G_PROGMEM = {0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x18, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x1f, 0x80, 0x00, 0x1f, 
	0x80, 0x00, 0x1f, 0x80, 0x00, 0x1f, 0x80, 0x00, 0x1f, 0x81, 0x00, 0x07, 0x83, 0xc0, 0x01, 0x8f, 
	0xf0, 0x00, 0x3f, 0xf8, 0x00, 0xbf, 0xf8, 0x01, 0x8f, 0xe0, 0x07, 0x83, 0xc0, 0x1f, 0x81, 0x00, 
	0x1f, 0x80, 0x00, 0x1f, 0x80, 0x00, 0x1f, 0x80, 0x00, 0x1f, 0x80, 0x00, 0x1e, 0x00, 0x00, 0x18, 
	0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00
};



void draw(void) {
  // graphic commands to redraw the complete screen should be placed here  
  u8g.drawBitmapP(52, 20, 3, 24,rook_bitmap);
}

void loop() {
  // Start a new measurement:
  digitalWrite(PIN_TRIG1, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG1, LOW);
  int duration0 = pulseIn(PIN_ECHO, HIGH);

  digitalWrite(PIN_TRIG2, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG2, LOW);
  int duration1 = pulseIn(PIN_ECHO, HIGH);

    digitalWrite(PIN_TRIG3, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG3, LOW);
  int duration2 = pulseIn(PIN_ECHO, HIGH);

  // Read the result:
  Serial.print("Distance0 in CM: ");
  Serial.println(duration0/ 58);
  int distance0 = (duration0 / 58);

    Serial.print("Distance1 in CM: ");
  Serial.println(duration1/ 58);
  int distance1 = (duration1 / 58);

    Serial.print("Distance2 in CM: ");
  Serial.println(duration2/ 58);
  int distance2 = (duration2 / 58);
  

  u8g.firstPage();
  do {
    draw();
    //top right
    if(distance0<beginDraw)
u8g.drawDisc(124,0,(beginDraw-distance0)/4);
  //middle right
  if(distance1<beginDraw)
u8g.drawDisc(128,32,(beginDraw-distance1)/4);
  //bottom right
  if(distance2<beginDraw)
  u8g.drawDisc(124,64,(beginDraw-distance2)/4);

    u8g.drawRFrame(40,20,50,25,2);
if(distance0<30 ||distance1<30 ||distance2<30)
u8g.drawStr(35, 64, "!WARNING!");
  } while ( u8g.nextPage() );



}