/* IRRemote teek vajab dekodeerimise makrot enne teegi lisamist */
#define DECODE_SAMSUNG

#include <IRremote.hpp>
#include <Servo.h>

#define BUTTON_PIN              2

#define SONIC_ECHO_PIN          3
#define SONIC_TRIG_PIN          4

#define RIGHT_SERVO_PIN         5
#define LEFT_SERVO_PIN          6

#define LEFT_LED_PIN            7
#define RIGHT_LED_PIN           8

#define SERVO_MIN_PULSE      1300
#define SERVO_MAX_PULSE      1700
#define SERVO_STANDSTILL     1500

Servo leftWheel;
Servo rightWheel;

bool robotEnabled = true;

void setup()
{
  Serial.begin(9600);

  /* Konfigureerime servomootorid */
  leftWheel.attach(LEFT_SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  rightWheel.attach(RIGHT_SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
   
  SetWheels(SERVO_STANDSTILL, SERVO_STANDSTILL);
}

void loop()
{
  /* Kui robot on väljas, siis IR vastuvõttu ei lähe */
  if (!robotEnabled)
  {
    return;
  }

  ReceiveIr();
}

void ReceiveIr(void)
{
  if (IrReceiver.decode()) 
  {
    if (IrReceiver.decodedIRData.protocol == SAMSUNG) 
    {
      IrReceiver.printIRResultShort(&Serial);

      /* Edastame käsukoodi funktsiooni, mis selle lahendab */
      ProcessCommand(IrReceiver.decodedIRData.command);
    }

    /* Jätkame IR vastuvõttu alles pärast käsu täitmist, vähendab korduste lugemist */
    IrReceiver.resume();
  }
}

void ProcessCommand(uint16_t cmd)
{
  /* Tuvastame puldilt vastuvõetud käsukoodi */
  switch (cmd)
  {
    case /* Asenda käsukoodiga puldilt */:
      /* Tegevused vastava käsu jaoks */
    default:
      break;
  }
}

void MoveBack(uint16_t duration)
{
  SetWheels(1400, 1600);
  delay(duration);
}

void MoveForward(uint16_t duration)
{
  SetWheels(1600, 1400);
  delay(duration);
}

void SetWheels(int leftWheelImpulseLength, int rightWheelImpulseLength)
{
  leftWheel.writeMicroseconds(leftWheelImpulseLength);
  rightWheel.writeMicroseconds(rightWheelImpulseLength);
}

float GetDistInCm() 
{
  /* Saadame ultrahelilaine välja, kestvus 10 us */
  digitalWrite(SONIC_TRIG_PIN, HIGH);   
  delayMicroseconds(10);                
  digitalWrite(SONIC_TRIG_PIN, LOW);    

  /* Loeme signaali peegeldumise aja */
  uint16_t echoTime = pulseIn(SONIC_ECHO_PIN, HIGH);
  
  /* Tagastame aja teisendatud kauguseks */
  return (echoTime / 58.0); 
}