#include "TagVis_Integration_JIG.h"

int pinMotors[QNT_SERVO_MOTORS]={4,5,6,7,8};  
int pinLeds[QNT_LEDS]={A0,A1};
int pinButtons[QNT_BUTTONS]={2,9,10,11,12,13}; 



Servo       tServo[QNT_SERVO_MOTORS];
ezButton    buttons[QNT_BUTTONS]= {
                                    ezButton(pinButtons[ALL_BUTTON]), 
                                    ezButton(pinButtons[POS_1_BUTTON]),
                                    ezButton(pinButtons[POS_2_BUTTON]),
                                    ezButton(pinButtons[POS_3_BUTTON]),
                                    ezButton(pinButtons[POS_4_BUTTON]),
                                    ezButton(pinButtons[POS_5_BUTTON])
                                  };

Load        leds[QNT_LEDS] =      {
                                    Load(pinLeds[FREE_LED],HIGH),
                                    Load(pinLeds[BUSY_LED],HIGH)
                                  }; 

static enum { TESTING, WAIT_START } state = WAIT_START;

#if DRIVER_TYPE == INTERNAL_SERVO

boardDef jigaBoardDef = {
    {4,5,6,7,8}, //pin motors  
    {A0,A1},     // pin leds 
    {2,9,10,11,12,13}, //pin buttons
    {
      &tServo[POS_1_MOTOR],
      &tServo[POS_2_MOTOR],
      &tServo[POS_3_MOTOR],
      &tServo[POS_4_MOTOR],
      &tServo[POS_5_MOTOR]
    },
    {
      &buttons[ALL_BUTTON],
      &buttons[POS_1_BUTTON],
      &buttons[POS_2_BUTTON],
      &buttons[POS_3_BUTTON],
      &buttons[POS_4_BUTTON],
      &buttons[POS_5_BUTTON]
    },
    {
      &leds[FREE_LED],
      &leds[BUSY_LED]
    }

};   

#endif

#if DRIVER_TYPE == ADAFRUIT_PWM_SERVO

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

boardDef jigaBoardDef = {
    {4,5,6,7,8}, //pin motors  
    {A0,A1},     // pin leds 
    {2,9,10,11,12,13}, //pin buttons
    {
      &pwm,
      &pwm,
      &pwm,
      &pwm,
      &pwm
    },
    {
      &buttons[ALL_BUTTON],
      &buttons[POS_1_BUTTON],
      &buttons[POS_2_BUTTON],
      &buttons[POS_3_BUTTON],
      &buttons[POS_4_BUTTON],
      &buttons[POS_5_BUTTON]
    },
    {
      &leds[FREE_LED],
      &leds[BUSY_LED]
    }

};   

#endif



jigTagvisIntegration  jiga = jigTagvisIntegration(&jigaBoardDef);  


void setup() {
  Serial.begin(9600);
  delay(100);
  
  
  // put your setup code here, to run once:
  Serial.print("jiga init\r\n");
  jiga.init();
  Serial.print("jiga inited\r\n");
  
  
}

void loop() {

  jiga.loop();
  
  switch(state)
  {
    case WAIT_START:
      
      for(byte i=0;i<QNT_BUTTONS;i++)
      {
        
        if (jiga.buttons[i]->isPressed())
        {

          state = TESTING;
          jiga.leds[FREE_LED]->setOff();
          jiga.leds[BUSY_LED]->setOn();
          Serial.print("Check button ");
          Serial.println(i);
          if(i == ALL_BUTTON)
          {
            Serial.print("ALL BUTTON PRESSED\r\n");
            jiga.pulseAll();          
          }
          else
          {
            Serial.print("BUTTON POS: ");
            Serial.print(i);
            Serial.print("  PRESSED\r\n");
            jiga.pulseServoPosition(i-1);          
      
          }

          break;  
        } 
      }  
            
      
    break;
    case TESTING:   // wait for the delay period
      if (jiga.isBusy())
      {
          //do nothing  
      }
      else
      {
        Serial.print("FREE\r\n");
        state = WAIT_START;
        jiga.leds[FREE_LED]->setOn();
        jiga.leds[BUSY_LED]->setOff();
        
      }
    break;
  }


}