/*************************************************** 
  This is an example for our Adafruit 16-channel PWM & Servo driver
  Servo test - this will drive 8 servos, one after the other on the
  first 8 pins of the PCA9685

  Pick one up today in the adafruit shop!
  ------> http://www.adafruit.com/products/815
  
  These drivers use I2C to communicate, 2 pins are required to  
  interface.

  Adafruit invests time and resources providing this open source code, 
  please support Adafruit and open-source hardware by purchasing 
  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.  
  BSD license, all text above must be included in any redistribution
 ****************************************************/

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);

// Depending on your servo make, the pulse width min and max may vary, you 
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN  200 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  400 // This is the 'maximum' pulse length count (out of 4096)

//USMIN,USMAX are pulse length values in microseconds which are only required if you use writeMicroseconds() rather than the conventional setPWM().
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

// our servo # counter
uint8_t servonum = 0;

// bity
bool bit1 = false;
bool bit2 = false;
bool bit3 = false;
bool bit4 = false;
char str[20];
int liczba_dziesietna;



/*
// Definiujemy piny do których podłączone są przyciski i limit switchy
const int limitSwitch1Pin = 2; // Limit Switch 1 - Reverse
const int pushButton1Pin = 3;  // Push Button (TO-GA 1)
const int pushButton2Pin = 4;  // Push Button (AT Disengage-1)
const int limitSwitch2Pin = 5; // Limit Switch 2 - Reverse
const int pushButton3Pin = 6;  // Push Button (TO-GA 2)
const int pushButton4Pin = 7;  // Push Button (AT Disengage-2)
// Zmienne do przechowywania stanu przycisków i limit switchy
bool limitSwitch1State = false;
bool pushButton1State = false;
bool pushButton2State = false;
bool limitSwitch2State = false;
bool pushButton3State = false;
bool pushButton4State = false;
void setup() {
  // Ustawiamy piny jako wejścia
  pinMode(limitSwitch1Pin, INPUT_PULLUP);
  pinMode(pushButton1Pin, INPUT_PULLUP);
  pinMode(pushButton2Pin, INPUT_PULLUP);
  pinMode(limitSwitch2Pin, INPUT_PULLUP);
  pinMode(pushButton3Pin, INPUT_PULLUP);
  pinMode(pushButton4Pin, INPUT_PULLUP);
  // Ustawiamy komunikację szeregową do debugowania
}

*/


void setup() {
  Serial.begin(9600);
  Serial.println("8 channel Servo test!");

  pwm.begin();
  /*
   * In theory the internal oscillator (clock) is 25MHz but it really isn't
   * that precise. You can 'calibrate' this by tweaking this number until
   * you get the PWM update frequency you're expecting!
   * The int.osc. for the PCA9685 chip is a range between about 23-27MHz and
   * is used for calculating things like writeMicroseconds()
   * Analog servos run at ~50 Hz updates, It is importaint to use an
   * oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip.
   * 1) Attach the oscilloscope to one of the PWM signal pins and ground on
   *    the I2C PCA9685 chip you are setting the value for.
   * 2) Adjust setOscillatorFrequency() until the PWM update frequency is the
   *    expected value (50Hz for most ESCs)
   * Setting the value here is specific to each individual I2C PCA9685 chip and
   * affects the calculations for the PWM update frequency. 
   * Failure to correctly set the int.osc value will cause unexpected PWM results
   */
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);
  pinMode(5, INPUT_PULLUP);
  pinMode(13, OUTPUT);


  delay(10);
}

// You can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= SERVO_FREQ;   // Analog servos run at ~60 Hz updates
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000000;  // convert input seconds to us
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);



}

void loop() {
  // Drive each servo one at a time using setPWM()

bit1 = digitalRead(2);
bit2 = digitalRead(3);
bit3 = digitalRead(4);
bit4 = digitalRead(5);

liczba_dziesietna = bit4 * 8 + bit3 * 4 + bit2 * 2 + bit1 * 1;

/*

    if (liczba_dziesietna == 1) {
      Serial.println("-->");
    }else if (liczba_dziesietna == 2) {
      Serial.println("<--");
    }else{
      Serial.println("--");
    }
*/
    switch (liczba_dziesietna)
    {
    case 1:
        Serial.println("-->");
        break;
    case 2:
        Serial.println("<--");
        break;
    case 3:
        Serial.println("^");
        break;
    case 4:
        Serial.println("v");
        break;
    case 5:
        Serial.println("Z ^");
        break;
    case 6:
        Serial.println("Z v");
        break;
    case 10:
        Serial.println("10");
        pwm.setPWM(0, 0, 110);
        break;
    case 11:
        Serial.println("11");
        pwm.setPWM(0, 0, 184);
        break;
    case 12:
        Serial.println("11");
        pwm.setPWM(0, 0, 258);
        break;
    case 13:
        Serial.println("11");
        pwm.setPWM(0, 0, 332);
        break;
    case 14:
        Serial.println("11");
        pwm.setPWM(0, 0, 406);
        break;
    case 15:
        Serial.println("11");
        pwm.setPWM(0, 0, 480);
        break;
    default:
        Serial.println("--");
        break;
    }


  if (bit1 == HIGH) {
    digitalWrite(13, HIGH);
  } else {
    digitalWrite(13, LOW);
  }


//Serial.println(sensorValue, DEC);

sprintf(str, "%d %d %d %d [%d]", bit4, bit3, bit2, bit1, liczba_dziesietna);

Serial.println(str);





delay(500);

/*
  Serial.println("teraz setPWM");
  Serial.println(servonum);
  for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {
    pwm.setPWM(servonum, 0, pulselen);
    Serial.print(servonum);
    Serial.print(" PULSELEN=");
     Serial.println(pulselen);
    delay(15);
  }

  delay(500);
  for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) {
    pwm.setPWM(servonum, 0, pulselen);
    delay(15);
  }

  delay(500);
Serial.println("a teraz w milisek");
  // Drive each servo one at a time using writeMicroseconds(), it's not precise due to calculation rounding!
  // The writeMicroseconds() function is used to mimic the Arduino Servo library writeMicroseconds() behavior. 
  for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
    pwm.writeMicroseconds(servonum, microsec);
  }

  delay(500);
  for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
    pwm.writeMicroseconds(servonum, microsec);
  }

  delay(500);

  servonum++;
  if (servonum > 2) servonum = 0; // Testing the first 8 servo channels
  */


}