#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� kolo
#define RIGHT_MOTOR_PIN_E 7    // prav� kolo

#define LEFT_MOTOR_PIN_A 22    // lev� kolo - ur�en� sm�ru A
#define LEFT_MOTOR_PIN_B 24    // lev� kolo - ur�en� sm�ru B
#define RIGHT_MOTOR_PIN_A 23   // prav� ...
#define RIGHT_MOTOR_PIN_B 25   // prav� ...

#define MOTOR_MIN_SPEED 30  // minim�ln� rychlost pojezdu

#define STEER_SERVO_PIN 8 // zat��en�
#define DRIVE_SERVO_PIN 9 // n�hon
#define LIFT_SERVO_PIN 10 // klepeta

#define LIFT_SERVO_MIN 68
#define LIFT_SERVO_MAX 168

#define STEER_SERVO_MIN 110
#define STEER_SERVO_MAX 176

#define DRIVE_SERVO_MIN 7

#define FRONT_LIGHT_PIN 52
#define BACK_LIGHT_PIN 53
#define TOP_LIGHT_PIN 50
#define TOP_LIGHT_PIN_G 51
#define TOP_LIGHT_INTERVAL 500 // ms

/*****************************************************************************/
/**
 * Control units
 */
Servo servoSteer;
Servo servoLift;
Servo servoDrive;

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;

boolean isTopLightOn;
unsigned long lastTopLightTime;
int lastTopLightState;

/*****************************************************************************/
/**
 * Setup
 */
void setup()
{
  // set default values
  servoSteer.attach(STEER_SERVO_PIN);
  servoLift.attach(LIFT_SERVO_PIN);
  servoDrive.attach(DRIVE_SERVO_PIN);

  servoDrive.write(90 + DRIVE_SERVO_MIN);
  servoSteer.write(STEER_SERVO_MIN + (STEER_SERVO_MAX- STEER_SERVO_MIN) / 2);
  servoLift.write(LIFT_SERVO_MIN + (LIFT_SERVO_MAX - LIFT_SERVO_MIN) / 2);

  // lights
  pinMode(FRONT_LIGHT_PIN, OUTPUT); digitalWrite(FRONT_LIGHT_PIN, LOW);
  pinMode(BACK_LIGHT_PIN, OUTPUT); digitalWrite(BACK_LIGHT_PIN, LOW);
  pinMode(TOP_LIGHT_PIN, OUTPUT); digitalWrite(TOP_LIGHT_PIN, LOW);
  pinMode(TOP_LIGHT_PIN_G, OUTPUT); digitalWrite(TOP_LIGHT_PIN_G, LOW);    
  isTopLightOn = false;

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

/*****************************************************************************/
/**
 * Main loop
 */
void loop()
{
  // loops
  xbee.loop();
  if( isTopLightOn ) blinkTopLight();

  // 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:

        // motors
        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 / 2;
        }
        else{
          leftMotorOffset = 0;                
          rightMotorOffset = (180 - angle) / 2 / 2;
        }

        motorLeft.changeMotorSpeed(motorSpeed - leftMotorOffset); 
        motorRight.changeMotorSpeed(motorSpeed - rightMotorOffset);              

        // servo
        Serial.print("Steer: "); 
        Serial.println(  map(command.value, 0, 1023, 0 ,179) , DEC); // debug
        angle = map(command.value, 0, 1023, STEER_SERVO_MIN, STEER_SERVO_MAX);
        servoSteer.write(angle);

        break;

        // servo lift
      case REM_COMMNAND_TRIMMER5:
        Serial.print("Lift: "); 
        Serial.println(  map(command.value, 0, 1023, 0 ,179) , DEC); // debug
        angle = map(1023 - command.value, 0, 1023, LIFT_SERVO_MIN, LIFT_SERVO_MAX);
        servoLift.write(angle);
        break;   

        // servo drive
      case REM_COMMNAND_TRIMMER4:
        Serial.print("Drive: "); 
        Serial.println(  map(command.value, 0, 1023, 0 ,179) , DEC); // debug
        angle = map(command.value, 0, 1023, 0, 90 - DRIVE_SERVO_MIN);
        servoDrive.write(90 + DRIVE_SERVO_MIN + angle);
        break;   

      // front lights
      case REM_COMMNAND_SWITCH2:
         digitalWrite( FRONT_LIGHT_PIN, command.value & 0x1 ); break;
              
      // back lights
      case REM_COMMNAND_SWITCH4:
          digitalWrite( BACK_LIGHT_PIN, command.value & 0x1 ); break;

      // top light
      case REM_COMMNAND_SWITCH3:            
          digitalWrite( TOP_LIGHT_PIN, command.value & 0x1 ); break;              
      case REM_COMMNAND_SWITCH5:
           isTopLightOn = (command.value==1);
           if( !isTopLightOn )  digitalWrite(TOP_LIGHT_PIN, LOW); // turn off
           break;

        // END SWITCH              
      }
    }
  }    
}

/*****************************************************************************/
/**
 * Blink 
 */
void blinkTopLight()
{
    unsigned long t = millis();

    // it interval time elapsed, change state
    if( t - lastTopLightTime >= TOP_LIGHT_INTERVAL)
    {
      lastTopLightTime = t;
      lastTopLightState = 1 - lastTopLightState; // negate
      digitalWrite(TOP_LIGHT_PIN, lastTopLightState);
    }
}