#include <Motor.h>
#include <Switch.h>
#include <IrDekoder.h>
#include <OvlServa.h>

Motor levy(30,32,3);
Motor magnet(31,33,2);
Motor zataceni(50,52,4);
Motor otoceni(51,53,5);

int motorPeriod=1000;

OvlServa maly(8);
OvlServa prostredni(9);
OvlServa velky(10);

IrDekoder ir(40);

//
int s1=22;
int s2=23;

/**********************************/
void setup()
{
  pinMode(s1,INPUT);
  pinMode(s2,INPUT);
  
  otoceni.setMotorSpeed(90);
  zataceni.setMotorSpeed(90);  

  //servo init  
  maly.attach(); prostredni.attach(); velky.attach();
  initPosition();
  
  Serial.begin(9600);
}

/**********************************/
void loop()
{
  //rotate
  int a=digitalRead(s1);
  int b=digitalRead(s2);
  
  if( otoceni.getMotorDirection()==-1 && a==0 )otoceni.stopMotor();
    else if( otoceni.getMotorDirection()==1 && b==0 )otoceni.stopMotor();
 
  //servo
  maly.servoLoop(); prostredni.servoLoop(); velky.servoLoop();
  
  //serial
  if( Serial.available()>0 )
  {
    switch( Serial.read() )
    {
      case 'a':otoceni.forward();break;
      case 'b':otoceni.backward();break;
      case 'c':otoceni.stopMotor();break;
      
      case 'd':
        zataceni.forward();
        delay(motorPeriod);
        zataceni.stopMotor();
        break;
      case 'e':
        zataceni.backward();
        delay(motorPeriod);
        zataceni.stopMotor();
        break;
   
      case 'f':dopredu();break;
      case 'g':dozadu();break;
      case 'h':zastavit();break;   
      
      //otoceni
      case 'A':vlevo();break;
      case 'B':vpravo();break;  
  
      //serva
      case 'X': case 'x':
          maly.abort(); prostredni.abort(); velky.abort();
          zataceni.stopMotor(); levy.stopMotor(); otoceni.stopMotor(); magnet.stopMotor(); //pravy.stopMotor();
          break;    
          
      case 'i': initPosition();break;
      case 'j': pickPosition();break;      
      case 'k': releasePosition();break;       
 
      case 'l': nalozit();break;     
      case 'm': vylozit();break;     

      case 'n': nalozitVylozit();break;      
   
    }
  }
  
  //IR
  if( ir.recievedData() )
  {
    switch(ir.getRecievedData())
    {
       //POJEZD
        case 594: //nahoru DOPREDU
          dopredu();
          break;
          
        case 338: //dolu DOZADU
          dozadu();
          break;
            
        case 1002: //menu STOP pojezd
            zastavit();
            break;     
            
        case 146: //doleva DOLEVA
          zataceni.forward();
          delay(motorPeriod);
          zataceni.stopMotor();       
          break;
  
        case 658: //doprava DOPRAVA
          zataceni.backward();
          delay(motorPeriod);
          zataceni.stopMotor();      
          break;        
          
       //NALOZENI
       case 98:
         nalozitVylozit();
         break;
    }
  }
  
}

/**********************************/
void nalozitVylozit()
{
  nalozit();
  vylozit();
}

/**********************************/
void nalozit()
{
    Serial.println("LOAD");
    
    //rotace vlevo
    otoceni.backward();
    while( !rotovat() ){}    
  
    //nahoru
    initPosition();
    while(true){ maly.servoLoop(); prostredni.servoLoop(); velky.servoLoop(); if( nastaveno() )break; }
    
    //dolu
    pickPosition();
    while(true){ maly.servoLoop(); prostredni.servoLoop(); velky.servoLoop(); if( nastaveno() )break; }
    
    //PICK
    magnet.forward();
    delay(500);
    
    //nahoru
    initPosition();
    while(true){ maly.servoLoop(); prostredni.servoLoop(); velky.servoLoop(); if( nastaveno() )break; }
    
    Serial.println("LOADED");
}

/**********************************/
void vylozit()
{
    Serial.println("UNLOAD");
    
    //rotace vlevo
    otoceni.forward();
    while( !rotovat() ){}    
  
    //nahoru
    initPosition();
    while(true){ maly.servoLoop(); prostredni.servoLoop(); velky.servoLoop(); if( nastaveno() )break; }
    
    //dolu
    releasePosition();
    while(true){ maly.servoLoop(); prostredni.servoLoop(); velky.servoLoop(); if( nastaveno() )break; }
    
    //RELEASE
    magnet.stopMotor();
    
    //nahoru
    initPosition();
    while(true){ maly.servoLoop(); prostredni.servoLoop(); velky.servoLoop(); if( nastaveno() )break; }
    
    Serial.println("UNLOADED");
}

/**********************************/
boolean nastaveno()
{
  if( maly.isMoving() || prostredni.isMoving() || velky.isMoving() )return false;
  
  return true;
}

/**********************************/
boolean rotovat()
{
  //rotate
  int a=digitalRead(s1);
  int b=digitalRead(s2);
  
  if( otoceni.getMotorDirection()==-1 && a==0 ){ otoceni.stopMotor(); return true; }
    else if( otoceni.getMotorDirection()==1 && b==0 ){ otoceni.stopMotor();return true; }
    
  return false;    
}

/**********************************/
void initPosition()
{
    maly.moveToPosition(120,100);
    prostredni.moveToPosition(160,100);
    velky.moveToPosition(110,100);          
}

/**********************************/
void pickPosition()
{
    maly.moveToPosition(140,100);
    prostredni.moveToPosition(130,100);
    velky.moveToPosition(175,100);          
}

/**********************************/
void releasePosition()
{
    maly.moveToPosition(140,100);
    prostredni.moveToPosition(150,100);
    velky.moveToPosition(150,100);          
}

/**********************************/
void vlevo()
{
  otoceni.backward();
  while( digitalRead(s1)== 1){}
  otoceni.stopMotor();
}

/**********************************/
void vpravo()
{
  otoceni.forward();
  while( digitalRead(s2)== 1){}
  otoceni.stopMotor();
}

/**********************************/
void dopredu()
{
  levy.forward();
 // pravy.forward();
}

/**********************************/
void dozadu()
{
  levy.backward();
 // pravy.backward();
}

/**********************************/
void zastavit()
{
  levy.stopMotor();
  //pravy.stopMotor();
}