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

/*****************************************************************************/
/**
 * Defines
 */
#define REEL_MOTOR_PIN_E 2    // naviják - enable
#define MOVE_MOTOR_PIN_E 3    // pojezd - enable

#define REEL_MOTOR_PIN_A 22   // naviják - určení směru A
#define REEL_MOTOR_PIN_B 24   // naviják - určení směru B
#define MOVE_MOTOR_PIN_A 23   // pojezd ...
#define MOVE_MOTOR_PIN_B 25   // pojezd ...

#define ROTATE_SERVO_PIN 4 // otáčení palet
#define TILT_SERVO_PIN   5 // náklon kolejnice
#define STEER_SERVO_PIN  6 // zatáčení kol

#define ROTATE_SERVO_CENTER 96
#define ROTATE_SERVO_MIDDLE_GAB 64
#define ROTATE_SERVO_SPEED 10 // 0 .. 90

#define TILT_SERVO_MAX 179 // not linear
#define TILT_SERVO_CENTER 123
#define TILT_SERVO_MIN 32

#define STEER_SERVO_MAX 124 // linear
#define STEER_SERVO_CENTER 92
#define STEER_SERVO_MIN 52

#define LIGHTS_FRONT    52 // přední světla - aktivní v HIGH
#define LIGHTS_REAR     53 // zadní světla - aktivní v HIGH

#define LIGHTS_TOP      48 // on pin
#define LIGHTS_TOP_GND  50 // gnd pin
#define LIGHTS_TOP_INTERVAL 500 // ms

/*****************************************************************************/
/**
 * Control units
 */
Servo servoRotate;
Servo servoTilt;
Servo servoSteer;

Motor motorReel(REEL_MOTOR_PIN_A, REEL_MOTOR_PIN_B, REEL_MOTOR_PIN_E);
Motor motorMove(MOVE_MOTOR_PIN_A, MOVE_MOTOR_PIN_B, MOVE_MOTOR_PIN_E);

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

RemoteRX remote;
RemoteCommandValue command;

unsigned long lastTopLightTime;
int lastTopLightState;
boolean isTopLightOn;

/*****************************************************************************/
/**
 * Setup
 */
void setup()
{
    // set default values
    pinMode(LIGHTS_TOP, OUTPUT);
    pinMode(LIGHTS_TOP_GND, OUTPUT);
    digitalWrite(LIGHTS_TOP, LOW); digitalWrite(LIGHTS_TOP_GND, LOW);
    lastTopLightTime = 0; lastTopLightState = LOW; isTopLightOn = false;
    
    pinMode(LIGHTS_FRONT, OUTPUT);
    pinMode(LIGHTS_REAR, OUTPUT);
    digitalWrite(LIGHTS_FRONT, LOW); digitalWrite(LIGHTS_REAR, LOW);
    
    servoSteer.attach(STEER_SERVO_PIN);
    servoTilt.attach(TILT_SERVO_PIN);
    servoSteer.write(STEER_SERVO_CENTER);
    servoTilt.write(TILT_SERVO_CENTER);
    
    servoRotate.attach(ROTATE_SERVO_PIN);
    servoRotate.write(ROTATE_SERVO_CENTER);
  
    // 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 )
         {
            // front lights
            case REM_COMMNAND_SWITCH2:
              digitalWrite( LIGHTS_FRONT, command.value & 0x1 ); break;
              
            // back lights
            case REM_COMMNAND_SWITCH4:
              digitalWrite( LIGHTS_REAR, command.value & 0x1 ); break;

            // top light
            case REM_COMMNAND_SWITCH3:            
              digitalWrite( LIGHTS_TOP, command.value & 0x1 ); break;              
            case REM_COMMNAND_SWITCH5:
              isTopLightOn = (command.value==1);
              if( !isTopLightOn )  digitalWrite(LIGHTS_TOP, LOW); // turn off
              break;
            
            // steering
            case REM_COMMNAND_TRIMMER6:
              angle = 180 - map(command.value, 0, 1023, STEER_SERVO_MIN, STEER_SERVO_MAX); 
              Serial.print("Steer angle: "); Serial.println(angle);
              servoSteer.write(angle);
              break;
              
            // servo tilt
            case REM_COMMNAND_TRIMMER5:
              angle = map(command.value, 0, 1023, TILT_SERVO_MIN, TILT_SERVO_MAX);
              Serial.print("Tilt angle: "); Serial.println(angle);
              servoTilt.write(angle);
              break;   
   
            // reel UP
            case REM_COMMNAND_BUTTON1:
               dir = command.value & 0x1;
               if( dir==1 ) motorReel.forward(); else motorReel.stopMotor();
               break;

            // reel down   
            case REM_COMMNAND_BUTTON2:
               dir = command.value & 0x1;
               if( dir==1 ) motorReel.backward(); else motorReel.stopMotor();               
               break;
           
            // move
            case REM_COMMNAND_TRIMMER1: 
              angle = command.value;
              if( angle== 511)
              {                   
                  motorMove.stopMotor();
              }
              else if( angle < 511 ) 
              {
                  angle = 100 - map(angle, 0, 511, 0, 100);                
                  Serial.print("Backward [%]: "); Serial.println(angle);
                  motorMove.backward(angle);
              }
              else 
              {
                  angle = map(angle, 511, 1023, 0, 100);                
                  Serial.print("Forward [%]: "); Serial.println(angle);
                  motorMove.forward(angle);
              }
              
              break;
           
            // rotating +
            case REM_COMMNAND_BUTTON3:
              dir = command.value & 0x1;
              if( dir==1 ) servoRotate.write(ROTATE_SERVO_CENTER + ROTATE_SERVO_SPEED);
              else servoRotate.write(ROTATE_SERVO_CENTER);
              
              break;

            // rotating -
            case REM_COMMNAND_BUTTON4:
              dir = command.value & 0x1;
              if( dir==1 ) servoRotate.write(ROTATE_SERVO_CENTER - ROTATE_SERVO_SPEED);
              else servoRotate.write(ROTATE_SERVO_CENTER);

              break;
            // END SWITCH              
         }
     }
  }    
}

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

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