#include "Config.h"
#include "Target.h"

// Number of targets.
#define NUM_TARGETS 2

Config config;

Target* targets[NUM_TARGETS];

void setup() {
  Serial.begin(9600);
  // Initialize targets.
  initTargets();
  // Print pinout.
  // getTargetPinouts();
  // Test
  // test();
  targets[0]->getRgbLed().changeColor(0, 1, 0);
  targets[1]->getRgbLed().changeColor(0, 0, 1);
}

void test()
{
   // @todo debug to undestand why this affects led in target 1
  targets[0]->getLaser().off();

  Serial.println("Test 0");
  Serial.print(targets[1]->getId());
  Serial.print(" ");
  Serial.print(targets[1]->getLaser().getId());
  Serial.print(" status ");
  Serial.print(targets[1]->getLaser().getStatus());
  Serial.println("");

  // targets[0]->getLaser().blink(3000, 200, 2);
  // targets[1]->getLaser().blink(200, 200, 5);
  // targets[2]->getLaser().blink(200, 200, 5);
  targets[1]->getLaser().on();
  Serial.println("Test 1");
  Serial.print(targets[1]->getId());
  Serial.print(" ");
  Serial.print(targets[1]->getLaser().getId());
  Serial.print(" status ");
  Serial.print(targets[1]->getLaser().getStatus());
  Serial.println("");

  targets[1]->getRgbLed().changeColor(0, 0, 1);
  targets[1]->getRgbLed().blink(50, 400, 2);

  byte i;
  // Initialize N number of targets.
  for ( i = 0 ; i < NUM_TARGETS ; i ++ )
  {
    //targets[i]->getLaser().blink(150, 200, 5);
    // targets[i]->getRgbLed().changeColor(0, 0, 255);
    // targets[i]->getRgbLed().blink(100, 500, 2);
  }
}

void initTargets()
{
    // Initialize next port.
  byte pin = config.getFirstPort();

  byte i;
  // Initialize N number of targets.
  for ( i = 0 ; i < NUM_TARGETS ; i ++ )
  {
    targets[i] = new Target();
    targets[i]->setup(i, pin);
    pin = targets[i]->getLastPin() + 1;
  }
}

// Print pinout.
// @todo move to class.
void getTargetPinouts()
{
  byte i;
  Serial.print("Port Mapping:\n");
  for ( i = 0 ; i < NUM_TARGETS ; i ++ )  
  {  
    Serial.print( "Target: "); Serial.println( i + 1 );
    Serial.print( "  Servo: "); Serial.println(targets[i]->getServoPin());
    Serial.print( "  Laser: "); Serial.println(targets[i]->getLaser().getPin());

    byte* led_ports = targets[i]->getRgbLed().getPins();
    char s[15];
    sprintf(s, "R %d, G %d, B %d", led_ports[0], led_ports[1], led_ports[2]);
    Serial.print( "  RGB Led: "); Serial.println(s);

    Serial.print( "  IR Receiver: ");
    Serial.println();
  }  
}

void loop() {
  byte i;
  // Serial.println(-2);
  for ( i = 0 ; i < NUM_TARGETS ; i ++ )
  {
    // Cascade loop.
    // Serial.println("Loop");
    targets[i]->loop();
    // Serial.print(targets[i]->getId());
    // Serial.println(" ");
    // Serial.print(targets[i]->getLaser().getId());
    // Serial.print(" status ");
    // Serial.print(targets[i]->getLaser().getStatus());
    // Serial.println(" ");

    // Serial.print(targets[i]->getRgbLed().getId());
    // Serial.print(" status ");
    // byte* status = targets[i]->getRgbLed().getStatus();
    // Serial.print(status[0]);
    // Serial.print(",");
    // Serial.print(status[1]);
    // Serial.print(",");
    // Serial.print(status[2]);
    // Serial.println("");
  }

  // @todo use laser.blink()
  // targets[i]->laserPulse();
  // temporary
  // targets[i]->servoUp();
  // temp targets[4]->getLaser().blink(150, 200, 2);
  // temp targets[i]->getLaser().blink(150, 200, 2);
  // targets[0]->getLaser().on();
  // delay(100);
  // targets[1]->getLaser().off();
  // targets[0]->getLaser().blink(150, 200, 2);
  targets[1]->getLaser().blink(50, 400, 2);
  // targets[0]->getRgbLed().blink(150, 200, 2);
  // targets[1]->getRgbLed().blink(50, 400, 2);

  // delay(1000);
  for ( i = 0 ; i < NUM_TARGETS ; i ++ )
  {
    // temporary
    // targets[i]->servoDown();
  }
  // delay(1000);
  delay(10);
  // Serial.println(2);
}