#include <Servo.h>
struct Motor {
  int min_angle, max_angle;
  Servo servo;
};
#define IMG_W 64 
#define IMG_H 64 
int STEP = 2;    
#define IDLING_MS 4 
#define WRITING_MS 8
struct Motor motor_x, motor_y, motor_p;
byte img[IMG_W/8]; 
                   
struct Motor newMotor(int pin, int a1, int a2) { 
  struct Motor m;
  m.servo.attach(pin);
  m.min_angle = a1;
  m.max_angle = a2;
  return m;
}
void gotoStart() {
   motor_p.servo.write(motor_p.max_angle); 
   delay(15);
   
   
   motor_x.servo.write(motor_x.min_angle);
   motor_y.servo.write(motor_y.min_angle); 
   delay(IDLING_MS * STEP * IMG_W + 150);
}
void finish() { 
    motor_p.servo.write(motor_p.max_angle); 
    delay(15);
    motor_x.servo.write(motor_x.min_angle);
    motor_y.servo.write(motor_y.max_angle);
    delay(IDLING_MS * STEP * IMG_W + 150);
}
void setup() { 
  Serial.begin(9600);
  motor_y = newMotor(2, 50, 180);
  motor_x = newMotor(4, 30, 160);
  motor_p = newMotor(6, 90, 99);
  gotoStart();
}
boolean nextWayIsEmpty(int i) {
  for(; i<IMG_W; ++i) {
    if(!(img[i/8] & (1 << 7-(i%8)))) continue;
    else return false;
  }
  
  return true;
}
void printImg() {
  gotoStart();
  int x = 0;
  for(int y=0; y<IMG_H; ++y) {
      for(int i=0; i<IMG_W/8; ++i) img[i] = Serial.read();
      Serial.write(61);
      
      for(int i=0; i<STEP; ++i) { 
          motor_x.servo.write(motor_x.min_angle); 
          motor_y.servo.write(motor_y.min_angle + y*STEP + i); 
          delay(IDLING_MS * STEP * x + 30);
          
          for(x=0; x<IMG_W; ++x) {
            
            if(nextWayIsEmpty(x)) {
              motor_p.servo.write(motor_p.max_angle); delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
              break;
            }
            
            
            motor_p.servo.write((img[x/8] & (1 << 7-(x%8))) ? motor_p.min_angle : motor_p.max_angle);
            delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
            
            
            motor_x.servo.write(motor_x.min_angle + x*STEP);
            delay(((img[x/8] & (1 << 7-(x%8))) ? WRITING_MS : IDLING_MS) * STEP); 
        }
        
        
        motor_p.servo.write(motor_p.max_angle); delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
      }
    
    
    motor_p.servo.write(motor_p.max_angle); delay(IDLING_MS * (motor_p.max_angle-motor_p.min_angle));
  }
    
  gotoStart();
  Serial.flush();
}
void checkMessage() {
    if(Serial.available()) {
      delay(10);
      byte msg[] = {Serial.read(), Serial.read()};
      
      if(msg[0] == 'P' && msg[1] == 'R') printImg(); 
      if(msg[0] == 'S' && msg[1] == 'T') gotoStart(); 
      if(msg[0] == 'C' && msg[1] == 'L') finish(); 
      if(msg[0] == 'S' && msg[1] == 'Z') STEP = Serial.read(); 
      if(msg[0] == 'P' && msg[1] == 'T') { 
        if(Serial.read() == '+') {
          byte b = Serial.read();
          motor_p.min_angle += b-'0';
          motor_p.max_angle += b-'0';
        } else {
          byte b = Serial.read();
          motor_p.min_angle -= b-'0';
          motor_p.max_angle -= b-'0';
        }
      }  
    }
}
void loop() {
  checkMessage();
}