#include <LiquidCrystal.h>
#include <Motor.h>
#include <Switch.h>

//Motors
Motor axisX(31,33,8);
Motor axisY(35,37,9);
Motor axisZ(32,34,10);

int YLpin=27;
int YRpin=29;
int XLpin=28;
int XRpin=30;

//switches, buttons
Switch bt1(44);
Switch bt2(46);
Switch bt3(48);
Switch bt4(50);
Switch sw1(45);
Switch sw2(47);
Switch sw3(49);
Switch sw4(51);
boolean s1,s2,s3,s4; //read

//proximity
int proxPin=A0;
int proxVal=0;
int pra,prb,prc,prd,pri=0;

//lcd
LiquidCrystal lcd(39,43,36,38,40,42); //RS EN;  (RW to ground)    ; data 5 6 7 8 
int lcdW=20;
int lcdH=4;

//menu variables
int actualMenu=0; //start
boolean redraw = true;

//other variables
boolean inited=false;
int helpLine;
int selectedMotor=0; //X
int motorDirection=0;

//include other files
#include "show.h"
#include "menu.h"

/******************************************************************************/
/**
 * SETUP
 */ 
void setup()
{
    pinMode(41,OUTPUT);digitalWrite(41,LOW); //RW for LCD
    lcd.begin(lcdW,lcdH);    
    
    axisZ.setMotorSpeed(90);
    pinMode(YLpin,INPUT);pinMode(YRpin,INPUT);
    pinMode(XLpin,INPUT);pinMode(XRpin,INPUT);
    
    Serial.begin(9600);
    pinMode(13,OUTPUT);digitalWrite(13,LOW);
}

/******************************************************************************/
/**
 * LOOP
 */ 
void loop()
{
    //MOTOR switches
    if( digitalRead(YLpin)==0 ){ if( axisY.getMotorDirection()==-1 )axisY.stopMotor(); }
    else if( digitalRead(YRpin)==0 ){ if( axisY.getMotorDirection()==1 )axisY.stopMotor(); }

    if( digitalRead(XLpin)==0 ){ if( axisX.getMotorDirection()==-1 )axisX.stopMotor(); }
    else if( digitalRead(XRpin)==0 ){ if( axisX.getMotorDirection()==1 )axisX.stopMotor(); }
  
    //read switches
    s1=sw1.isMomentaryPressed();
    s2=sw2.isMomentaryPressed();
    s3=sw3.isMomentaryPressed();
    s4=sw4.isMomentaryPressed();
    
    if( s1 && !s2 && !s3 && !s4 ){actualMenu=manualMenu;redraw=true;inited=false;}
    
    //which menu enter:
    switch( actualMenu )
    {
        case mainMenu:     showMainMenu(); break;
        case helpMenu:     showHelpMenu(); break;
        case manualMenu:   showManualMenu(); break;        
        case initMenu:     showInitMenu(); break;
        case findMenu:     showFindMenu(); break;        
    }
    //
}


/******************************************************************************/
/**
 * Set motor defult position
 */ 
 void init(int stopPin)
 {
    int a,b;
    int mx=1,my=1;
    boolean done=true;
    axisX.backward();
    axisY.backward();
    
    while(true)
    {
      a=digitalRead(XLpin);
      b=digitalRead(YLpin);
      if( stopPin>0 ){ if(digitalRead(stopPin)==1){done=false;break;} } //cancelled    
      
      if( a==0 && mx==1)
      {
        delay(1);
        if( digitalRead(XLpin)==0 ){mx=0;axisX.stopMotor();} //filter out noise
      }
      
      if( b==0 && my==1)
      {
        delay(1);
        if( digitalRead(YLpin)==0 ){my=0;axisY.stopMotor();} //filter out noisemy=0;axisY.stopMotor();}
      }
      
      if( mx==0 && my==0 )break; //done
    }
    
    //SUCCESS?
    if( done==false )
    {
      allStop();     
      return;
    }
    
    //OK
    inited=true;
 }
 
/******************************************************************************/
/**
 * All stop
 */
 void allStop()
 {
    axisX.stopMotor();
    axisY.stopMotor();
    axisZ.stopMotor();
 }
 
/******************************************************************************/
/**
 * Find
 */
 void findm(int stopPin)
 {
    inited=false;
    boolean done=true;
    int r;     
    
    //find - Y
    Serial.println("BEGIN Y");
    axisY.forward();
    while( true )
    {
       if( stopPin>0 ){ if(digitalRead(stopPin)==1){axisY.stopMotor();return;} } //cancelled 
       if( digitalRead(YRpin)==0 ){done=false;break; }//nothing found
       
       r=readProximity();
       Serial.println(r,DEC);
       if( r> 100 )break;
    }        
    axisY.stopMotor();
    if( !done )return;
    
    //find - X
    int m=r;
    Serial.println("BEGIN X");    
    axisX.forward();
    while( true )
    {
       if( stopPin>0 ){ if(digitalRead(stopPin)==1){axisX.stopMotor();return;} } //cancelled 
       if( digitalRead(XRpin)==0 ){done=false;break; }//nothing found
       
       r=readProximity();
       if( r>m )m=r;
       Serial.println(r,DEC);
       if(r>700 || (m-r>50) )break;
    }        
    axisX.stopMotor();
    if( !done )return;    
    
    //Z
    axisZ.backward();
    delay(3000);
    axisZ.stopMotor();
    axisZ.forward();
    delay(6000);
    axisZ.stopMotor();    
    
    //get back = init
    init(stopPin);
 }
 
 
/******************************************************************************/
/**
 * Proximity
 */
 int readProximity()
 {
   int r=analogRead(A0);
   
   if( pri==0 )pra=r;
      else if( pri==1 )prb=r;
      else if( pri==1 )prc=r;
      else if( pri==1 )prd=r;      
      
   pri++; if( pri==4 )pri=0;
    
   return (pra+prb+prc+prd)/4;
 }