#include <motor.h>
#include <myServo.h>
#include <xbee.h>
#include <remoteRX.h>
#include "servo.h"
#include <remote.h>

/*****************************************************************************/
/**
 * Defines
 */
#define LEFT_MOTOR_PIN_E 6     // levý pás
#define RIGHT_MOTOR_PIN_E 7    // pravý pás

#define LEFT_MOTOR_PIN_A 22    // levý pás - určení směru A
#define LEFT_MOTOR_PIN_B 24    // levý pás - určení směru B
#define RIGHT_MOTOR_PIN_A 23   // pravý ...
#define RIGHT_MOTOR_PIN_B 25   // pravý ...

#define ROTATE_SPEED 90     // rychlost otáčení pásáku na místě [%]
#define MOTOR_MIN_SPEED 30  // minimální rychlost pojezdu

#define LIFT_SERVO_PIN 8 // zvedání lžíce
#define TILT_SERVO_PIN 9 // náklon lžíce

/*****************************************************************************/
/**
 * Control units
 */
Servo servoLift;
Servo servoTilt;

Motor motorLeft(LEFT_MOTOR_PIN_A, LEFT_MOTOR_PIN_B, LEFT_MOTOR_PIN_E);
Motor motorRight(RIGHT_MOTOR_PIN_A, RIGHT_MOTOR_PIN_B, RIGHT_MOTOR_PIN_E);

XBee xbee(false, Serial1); // set to slave
unsigned int xbeeCommand[XBEE_SEND_COMMAND_LENGTH];

RemoteRX remote;
RemoteCommandValue command;

int leftMotorOffset = 0;
int rightMotorOffset = 0;
int motorSpeed = 0;

/*****************************************************************************/
/**
 * Setup
 */
void setup()
{
    // set default values
    servoLift.attach(LIFT_SERVO_PIN);
    servoTilt.attach(TILT_SERVO_PIN);

    // set serial for debugging
    Serial.begin(9600);
}

/*****************************************************************************/
/**
 * Main loop
 */
void loop()
{
  // loops
  xbee.loop();
  
  // check xbee
  unsigned long angle;
  int dir;

  if( xbee.isConnected() )
  {
     if( xbee.recievedCommand() )
     {
         xbee.getCommand(xbeeCommand);
         command = remote.parseRecievedValue( (xbeeCommand[0] << 8) | xbeeCommand[1] );
         
         Serial.print("Command: "); Serial.print(command.command); 
         Serial.print(", value: "); Serial.println(command.value); 
         
         // process command
         switch( command.command )
         {          
            // move
            case REM_COMMNAND_TRIMMER1: 
              angle = command.value;
              if( angle== 511)
              {                   
                 motorLeft.stopMotor();
                 motorRight.stopMotor();
              }
              else if( angle < 511 ) 
              {
                  angle = 100 - map(angle, 0, 511, 0, 100 - MOTOR_MIN_SPEED) + MOTOR_MIN_SPEED;                
                  motorSpeed = angle;
                  
                  motorLeft.forward(angle - leftMotorOffset); 
                  motorRight.forward(angle - rightMotorOffset);
              }
              else 
              {
                  angle = map(angle, 511, 1023, 0, 100 - MOTOR_MIN_SPEED) + MOTOR_MIN_SPEED;                
                  motorSpeed = angle;
                  
                  motorLeft.backward(angle - leftMotorOffset); 
                  motorRight.backward(angle - rightMotorOffset);
              }
              
              break;
                      
            // steering
            case REM_COMMNAND_TRIMMER6:
              angle = map(command.value, 0, 1023, 0, 180);
              if( angle >=85 && angle <= 95 )
              {
                  leftMotorOffset = 0;
                  rightMotorOffset = 0;
              }else if( angle >90){
                  rightMotorOffset = 0;
                  leftMotorOffset = angle / 2;
              }else{
                  leftMotorOffset = 0;                
                  rightMotorOffset = (180 - angle) / 2;              
              }
              
              motorLeft.changeMotorSpeed(motorSpeed - leftMotorOffset); 
              motorRight.changeMotorSpeed(motorSpeed - rightMotorOffset);              

              break;
              
            // servo tilt
            case REM_COMMNAND_TRIMMER5:
              angle = 180 - map(command.value, 0, 1023, 0, 180);
              servoTilt.write(angle);
              break;   
   
            // servo lift
            case REM_COMMNAND_TRIMMER4:
              angle = map(command.value, 0, 1023, 0, 180);
              servoLift.write(angle);
              break;   
   
            // rotate left
            case REM_COMMNAND_BUTTON1:
              dir = command.value & 0x1;
              if( dir == 0 )
              {
                 motorLeft.stopMotor();
                 motorRight.stopMotor();   
              }else{
                 motorLeft.forward(ROTATE_SPEED); 
                 motorRight.backward(ROTATE_SPEED);                               
              }
              
              break;
           
            // rotate right
            case REM_COMMNAND_BUTTON3:
              dir = command.value & 0x1;
              if( dir == 0 )
              {
                 motorLeft.stopMotor();
                 motorRight.stopMotor();   
              }else{
                 motorLeft.backward(ROTATE_SPEED); 
                 motorRight.forward(ROTATE_SPEED);                               
              }
              
              break;
            // END SWITCH              
         }
     }
  }    
}