#include <SoftwareServo.h>

SoftwareServo onOffServo;
SoftwareServo leftRightServo;
SoftwareServo upDownServo;

#define ON_OFF_SERVO_PIN PA6
#define LEFT_RIGHT_SERVO_PIN PA7
#define UP_DOWN_SERVO_PIN PB2

#define A 0
#define B 1

#define LASER_OFF_ANGLE 120
#define LASER_ON_ANGLE 140

#define MIN_UP_DOWN_ANGLE 25
#define MAX_UP_DOWN_ANGLE 47

#define MIN_LEFT_RIGHT_ANGLE 48
#define MAX_LEFT_RIGHT_ANGLE 132

// Y is 0 to 11
#define MIN_UP_DOWN_COORDINATE 0
#define MAX_UP_DOWN_COORDINATE (MAX_UP_DOWN_ANGLE-MIN_UP_DOWN_ANGLE)/2

#define MIN_Y MIN_UP_DOWN_COORDINATE
#define MAX_Y MAX_UP_DOWN_COORDINATE

// X is 0 to 40
#define MIN_LEFT_RIGHT_COORDINATE 0
#define MAX_LEFT_RIGHT_COORDINATE (MAX_LEFT_RIGHT_ANGLE-MIN_LEFT_RIGHT_ANGLE)/2

#define MIN_X MIN_LEFT_RIGHT_COORDINATE
#define MAX_X MAX_LEFT_RIGHT_COORDINATE

#define DRAW_PAUSE 500
#define DISPLAY_PAUSE 5000

#define DRAWING1_PIN PA0
#define DRAWING2_PIN PA1
#define DRAWING3_PIN PA2
#define DRAWING4_PIN PA3
#define DRAWING5_PIN PA4
#define DRAWING6_PIN PA5

int DRAWING_PINS[6] = {
  DRAWING1_PIN,
  DRAWING2_PIN,
  DRAWING3_PIN,
  DRAWING4_PIN,
  DRAWING5_PIN,
  DRAWING6_PIN
};

void setup()
{
  onOffServo.attach(ON_OFF_SERVO_PIN,A);
  leftRightServo.attach(LEFT_RIGHT_SERVO_PIN,A);
  upDownServo.attach(UP_DOWN_SERVO_PIN,B);

  for(int i = 0; i < 6; i++) {
    pinMode(DRAWING_PINS[i],INPUT);
    digitalWrite(DRAWING_PINS[i],HIGH); // pull down resistors
  }

  //Serial.begin(9600);

    laserOn();
}

void loop() {
  for(int i = 0; i < 5; i++) {
    if( digitalRead(DRAWING_PINS[i]) == HIGH ) {
      drawPicture(i);
      laserOff();
      delay(DISPLAY_PAUSE);
    }
  }
}

void drawPicture(int which) {
  if( which == 0 ) {
    fab();
  }
  else if( which == 1 ) {
    house();
  }
  else if( which == 2 ) {
    shaun();
  }
  else if( which == 3 ) {
    grid();
  }
  else if( which == 4 ) {
    face();
  }
//  else if( which == 5 ) {
//    squiggle();
//  }
}

void fab() {
  int X_OFFSET = 0;
  int Y_OFFSET = 0;
  int LETTER_LENGTH = 7;
  int LETTER_HEIGHT = MAX_Y;

  // F
  laserOff();
  moveTo(X_OFFSET,Y_OFFSET);
  laserOn();
  moveTo(X_OFFSET,Y_OFFSET + LETTER_HEIGHT);
  moveTo(X_OFFSET + LETTER_LENGTH,Y_OFFSET + LETTER_HEIGHT);
  laserOff();
  moveTo(X_OFFSET,Y_OFFSET + LETTER_HEIGHT/2);
  laserOn();
  moveTo(X_OFFSET + LETTER_LENGTH/2,Y_OFFSET + LETTER_HEIGHT/2);
  laserOff();

  // A
  moveTo(X_OFFSET + 2 * LETTER_LENGTH,Y_OFFSET);
  laserOn();
  moveTo(X_OFFSET + 2 * LETTER_LENGTH,Y_OFFSET + LETTER_HEIGHT);
  moveTo(X_OFFSET + 3 * LETTER_LENGTH,Y_OFFSET + LETTER_HEIGHT);
  moveTo(X_OFFSET + 3 * LETTER_LENGTH,Y_OFFSET);
  laserOff();
  moveTo(X_OFFSET + 2 * LETTER_LENGTH,Y_OFFSET + LETTER_HEIGHT/2);
  laserOn();
  moveTo(X_OFFSET + 3 * LETTER_LENGTH,Y_OFFSET + LETTER_HEIGHT/2);
  laserOff();

  // B
  moveTo(X_OFFSET + 5 * LETTER_LENGTH,Y_OFFSET);
  laserOn();
  moveTo(X_OFFSET + 5 * LETTER_LENGTH,Y_OFFSET+LETTER_HEIGHT);
  moveTo(X_OFFSET + 6 * LETTER_LENGTH,Y_OFFSET+LETTER_HEIGHT/4 * 3);
  moveTo(X_OFFSET + 5 * LETTER_LENGTH,Y_OFFSET+LETTER_HEIGHT/4 * 2);
  moveTo(X_OFFSET + 6 * LETTER_LENGTH,Y_OFFSET+LETTER_HEIGHT/4 * 1);
  moveTo(X_OFFSET + 5 * LETTER_LENGTH,Y_OFFSET);
}

void house() {
  int X_OFFSET = 0;
  int Y_OFFSET = 0;

  laserOff();

  // base
  moveTo(MAX_X/4,MIN_Y);
  laserOn();
  moveTo(MAX_X/4*3,MIN_Y);
  moveTo(MAX_X/4*3,MIN_Y+MAX_Y/2);
  moveTo(MAX_X/4,MIN_Y+MAX_Y/2);
  moveTo(MAX_X/4,MIN_Y);

  laserOff();

  // roof
  moveTo(MAX_X/4,MIN_Y+MAX_Y/2);
  laserOn();
  moveTo(MAX_X/2,MAX_Y);
  moveTo(MAX_X/4*3,MIN_Y+MAX_Y/2);
}

void shaun() {
  int LETTER_LENGTH = 4;
  int LETTER_HEIGHT = MAX_Y;
  int X_OFFSET = 4;

  laserOff();

  // S
  moveTo(X_OFFSET,MIN_Y);
  laserOn();
  moveTo(X_OFFSET+LETTER_LENGTH,MIN_Y);
  moveTo(X_OFFSET+LETTER_LENGTH,MIN_Y + MAX_Y/2);
  moveTo(X_OFFSET,MIN_Y + MAX_Y/2);
  moveTo(X_OFFSET,MAX_Y);
  moveTo(X_OFFSET+LETTER_LENGTH,MAX_Y);

  laserOff();

  moveTo(X_OFFSET+LETTER_LENGTH*2,MIN_Y);
  laserOn();
  moveTo(X_OFFSET+LETTER_LENGTH*2,MAX_Y);
  laserOff();
  moveTo(X_OFFSET+LETTER_LENGTH*3,MIN_Y);
  laserOn();
  moveTo(X_OFFSET+LETTER_LENGTH*3,MAX_Y);
  laserOff();
  moveTo(X_OFFSET+LETTER_LENGTH*2,MAX_Y/2);
  laserOn();
  moveTo(X_OFFSET+LETTER_LENGTH*3,MAX_Y/2);

  laserOff();

  // A
  moveTo(X_OFFSET + 4 * LETTER_LENGTH,MIN_Y);
  laserOn();
  moveTo(X_OFFSET + 4 * LETTER_LENGTH,MIN_Y + LETTER_HEIGHT);
  moveTo(X_OFFSET + 5 * LETTER_LENGTH,MIN_Y + LETTER_HEIGHT);
  moveTo(X_OFFSET + 5 * LETTER_LENGTH,MIN_Y);
  laserOff();
  moveTo(X_OFFSET + 4 * LETTER_LENGTH,MIN_Y + LETTER_HEIGHT/2);
  laserOn();
  moveTo(X_OFFSET + 5 * LETTER_LENGTH,MIN_Y + LETTER_HEIGHT/2);
  laserOff();

  // U
  moveTo(X_OFFSET + 6 * LETTER_LENGTH,MAX_Y);
  laserOn();
  moveTo(X_OFFSET + 6 * LETTER_LENGTH,MIN_Y);
  moveTo(X_OFFSET + 7 * LETTER_LENGTH,MIN_Y);
  moveTo(X_OFFSET + 7 * LETTER_LENGTH,MAX_Y);
  laserOff();

  // N
  moveTo(X_OFFSET + 8 * LETTER_LENGTH,MIN_Y);
  laserOn();
  moveTo(X_OFFSET + 8 * LETTER_LENGTH,MAX_Y);
  moveTo(X_OFFSET + 9 * LETTER_LENGTH,MIN_Y);
  moveTo(X_OFFSET + 9 * LETTER_LENGTH,MAX_Y);
}

void grid() {
  laserOff();

  moveTo(0,0);
  laserOn();

  for(int x = MIN_X; x <= MAX_X; x++) {
     moveTo(x,x % 2 == 0 ? MIN_Y : MAX_Y);
     moveTo(x,x % 2 == 0 ? MAX_Y : MIN_Y);
  }

  laserOff();

  moveTo(0,0);
  laserOn();

  for(int y = MIN_Y; y <= MAX_Y; y++) {
     moveTo(y % 2 == 0 ? MIN_X : MAX_X,y);
     moveTo(y % 2 == 0 ? MAX_X : MIN_X,y);
  }

}

//void squiggle() {
// laserOff();
//
// moveTo((int)(MAX_X * 0.05),MAX_Y);
// laserOn();
//
// for(double per = 0.05; per <= 1; per += 0.05) {
//   moveTo((int)(MAX_X * per),(int)(MAX_Y * (1 - 0)));
//   moveTo((int)(MAX_X * 0.0),(int)(MAX_Y * (1 - per)));
// }
//}

void face() {
 laserOff();

 // head
 moveTo(MAX_X/4,MAX_Y/4);
 laserOn();
 moveTo(MAX_X/4*3,MAX_Y/4);
 moveTo(MAX_X/4*3,8);
 moveTo(MAX_X/4,8);
 moveTo(MAX_X/4,MAX_Y/4);

 laserOff();

 // eyes / nose
 moveTo(MAX_X/3,MAX_Y/3*2);
 laserOn();
 pause();
 laserOff();
 moveTo(MAX_X/3*2,MAX_Y/3*2);
 laserOn();
 pause();
 laserOff();
 moveTo(MAX_X/2,MAX_Y/2);
 laserOn();
 pause();
 laserOff();

 // mouth
 moveTo(MAX_X/3,MAX_Y/3);
 laserOn();
 moveTo(MAX_X/3*2,MAX_Y/3);

}

void moveTo(int x, int y) {
  //Serial.print("moveTo(");
  //Serial.print(x);
  //Serial.print(",");
  //Serial.print(y);
  //Serial.println(");");
  moveToX(x);
  moveToY(y);
  pause();
}

void pause() {
   //Serial.println("pause();");
   //delay(DRAW_PAUSE);
   long now = millis();
   while(millis() - now < DRAW_PAUSE) {
     SoftwareServo::refresh();
   }
}

void moveToX(int x) {
  int angle = map(MAX_LEFT_RIGHT_COORDINATE-x,MIN_LEFT_RIGHT_COORDINATE,MAX_LEFT_RIGHT_COORDINATE,MIN_LEFT_RIGHT_ANGLE,MAX_LEFT_RIGHT_ANGLE);
  leftRightServo.write(angle);
}

void moveToY(int y) {
  int angle = map(y,MIN_UP_DOWN_COORDINATE,MAX_UP_DOWN_COORDINATE,MIN_UP_DOWN_ANGLE,MAX_UP_DOWN_ANGLE);
  upDownServo.write(angle);
}

void laserOn() {
  //Serial.println("laserOn();");
  onOffServo.write(LASER_ON_ANGLE);
  pause();
}

void laserOff() {
  //Serial.println("laserOff();");
  onOffServo.write(LASER_OFF_ANGLE);
  pause();
}