#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// declare pins for digital input from the RASPBERRY PI
/*/int x_up_pin = 2;
int x_down_pin = 3 ;
int y_up_pin = 4;
int y_down_pin = 5 ;/*/
// declare pins for servo output
int servo_x_Pin = 9;
int servo_y_Pin = 10;
// declare flag pin to send digital output to the RASPBERRY PI
//int signal_pin = 12;
// declare 2 servos to control the degrees of freedom of the XY Stage
Servo servo_x;
Servo servo_y;

/* Set the LCD address to 0x27 for a 16 chars and 2 lines display */
LiquidCrystal_I2C lcd(0x27, 16, 2);

// set up the default position of the servos: in the middle
int servo_x_Angle = 90;
int servo_y_Angle = 90;

// actuation flag: not moving
int actuating = false;
int moving_x = 0;
int moving_y = 0;
// Joystick sensitivity
int threshold = 64;
// Analog input pins for Joystick XY values
int JoyStick_X =0;// pin number for x
int JoyStick_Y =1;// pin number for y
void setup()
{
// communicate to RASPBERRY if actuating a stage move or listening
//pinMode(signal_pin, OUTPUT); // to send 1 bit message to pi
//digitalWrite(signal_pin, LOW);// not actuating
// prepare the servos
Serial.begin(9600);// 9600 bps
servo_x.attach(servo_x_Pin);
servo_y.attach(servo_y_Pin);
// prepare the LCD
lcd.init();
// Turn on the blacklight and print a message.
lcd.backlight();
lcd.setCursor(0,0);
lcd.print("OpenStage");
// position at the center of the scanning area (x,y)=(90,90)
servo_x.write(servo_x_Angle);
servo_y.write(servo_y_Angle);
// RASPBERRY communication pins (4 bits)
/*/pinMode(x_up_pin, INPUT); // GUI button x.step-up
pinMode(x_down_pin, INPUT); // GUI button x.step-down
pinMode(y_up_pin, INPUT); // GUI button y.step-up
pinMode(y_down_pin, INPUT); // GUI button y.step-down
delay(50);/*/
}
void loop()
{
// X movement (signal detection)
// x.up
if(analogRead(JoyStick_X) > (512 + threshold) && actuating == false)
                           //|| (digitalRead(x_up_pin)== HIGH && actuating == false))
            {
            if(servo_x_Angle < 180) servo_x_Angle++;
            actuating = true;
            }
//x.down
if(analogRead(JoyStick_X) < (512 - threshold) && actuating==false)
                           //||(digitalRead(x_down_pin)== HIGH && actuating == false))
           {
           if(servo_x_Angle > 0)servo_x_Angle--;
           actuating = true;
           }
// Y movement (signal detection)
//y.up
if(analogRead(JoyStick_Y) > (512 + threshold)&& actuating==false)
                           //||(digitalRead(y_up_pin)== HIGH && actuating==false))
           {
           if(servo_y_Angle < 180) servo_y_Angle++;
           actuating= true;
           }
//y.down
if(analogRead(JoyStick_Y) < (512 - threshold)&& actuating==false)
                           //||(digitalRead(y_down_pin)== HIGH && actuating==false))
           {
           if(servo_y_Angle > 0) servo_y_Angle--;
           actuating = true;
           }
// Move
if(actuating == true )
           {
           //digitalWrite(signal_pin, HIGH);  // actuating
           servo_x.write(servo_x_Angle);
           servo_y.write(servo_y_Angle);
           delay(50);
           actuating = false;
           //digitalWrite(signal_pin, LOW);  // not actuating
           }


lcd.setCursor(0,1);
lcd.print("position:");
lcd.print(servo_x_Angle); 
lcd.print(",");
lcd.print(servo_y_Angle); 
lcd.print("    ");
delay(50);
}