uint8_t digital_Setup[14] = { 0 };

void setup() {
  ledcSetup_mega2560(11, 15000, 1023);
  pinMode(11, OUTPUT);
}

void loop() {
  // analogWrite(11, 254);
  motorDrive_BTS7960(11, 254, 32, 34);
}

#ifdef __AVR_ATmega2560__

void ledcSetup_mega2560(int pin_PWM, int freq, int resolution_bits) {
  pinMode(pin_PWM, OUTPUT);
  if (resolution_bits == 8) {
    resolution_bits = 255;
  }
  if (resolution_bits == 10) {
    resolution_bits = 1023;
  }

  //Timer 1
  if ((pin_PWM == 11) || (pin_PWM == 12) || (pin_PWM == 13)) {
    TCCR1A = 0;
    TCCR1B = 0;
    //setup mode Fast PWM
    TCCR1A = (1 << COM1A1) | (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12);
    // TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);

    // Set freq
    if (resolution_bits == 1023) {
      if (freq == 15000) {
        TCCR1B |= (1 << CS10);  // Prescaler = 1
        ICR1 = 1066;
      }
      if (freq == 2000) {
        TCCR1B |= (1 << CS11);  // Prescaler = 8
        ICR1 = 999;
      }
      if (freq == 800) {
        TCCR1B |= (1 << CS11);  // Prescaler = 8
        ICR1 = 2300;
      }
    }
    if (resolution_bits == 255) {
      if (freq == 8000) {
        TCCR1B |= (1 << CS10);
        ICR1 = 2000;
      }
      if (freq == 12000) {
        TCCR1B = (0 << CS12) | (0 << CS11) | (1 << CS10);
        ICR1 = 166;
      }
    }
  }

  //Timer 3
  if ((pin_PWM == 2) || (pin_PWM == 3) || (pin_PWM == 5)) {
    //setup mode Fast PWM
    TCCR3A = (1 << COM3A1) | (1 << COM3B1) | (1 << COM3C1) | (1 << WGM31);
    TCCR3B = (1 << WGM33) | (1 << WGM32);

    // Set resolution_bits
    ICR3L = 0;
    ICR3H = 0;
    ICR3H = (uint8_t)(resolution_bits >> 8);
    ICR3L = (uint8_t)resolution_bits;

    // Set freq
    if (resolution_bits == 1023) {
      if (freq == 15000) {
        TCCR3B |= (1 << CS30);
      }
      if (freq == 2000) {
        TCCR3B |= (1 << CS31);
      }
    }
    if (resolution_bits == 255) {
      if (freq == 8000) {
        TCCR3B |= (1 << CS31);
      }
    }
  }

  //Timer 4
  if ((pin_PWM == 6) || (pin_PWM == 7) || (pin_PWM == 8)) {
    //setup mode Fast PWM
    // TCCR4A |= (1 << COM4A1) | (1 << COM4B1) | (1 << COM4C1) | (1 << WGM41);
    TCCR4A = (1 << COM4A1) | (1 << WGM41);
    TCCR4B = (1 << WGM43) | (1 << WGM42);
    // TCCR4B |= (1 << WGM43) | (1 << WGM42);

    // Set freq
    if (resolution_bits == 1023) {
      if (freq == 15000) {
        TCCR4B |= (1 << CS40);
        ICR4 = 1065;
      }
      if (freq == 2000) {
        TCCR4B |= (1 << CS41);
        ICR4 = 1000;
      }
    }
    if (resolution_bits == 255) {
      if (freq == 8000) {
        TCCR4B |= (1 << CS42) | (0 << CS41) | (0 << CS40);
        ICR4 = 12;
      }
    }
  }

  //Timer 2
  if ((pin_PWM == 9) || (pin_PWM == 10)) {
    //setup mode Fast PWM
  TCCR2A = (1 << WGM21) | (1 << WGM20); // 
  TCCR2B = (1 << CS21);                 // Prescaler = 8

  // Set non-inverting mode
  TCCR2A |= (1 << COM2A1) | (1 << COM2B1);

    // Set freq
    if (resolution_bits == 255) {
      if (freq == 8000) {
        TCCR2B |= (1 << CS21); //prescaler 8
      }
    }
  }
}

void motorDrive_BTS7960(uint8_t pin_PWM, int32_t duty, uint8_t pin_LPWM, uint8_t pin_RPWM) {
  uint16_t duty_16b = ((duty < 0) ? -duty : duty);

  switch (pin_PWM) {
    //Timer 1
    case 11:
      OCR1AH = (uint8_t)(duty_16b >> 8);
      OCR1AL = (uint8_t)duty_16b;
      break;

    case 12:
      OCR1BH = (uint8_t)(duty_16b >> 8);
      OCR1BL = (uint8_t)duty_16b;
      break;

    case 13:
      OCR1CH = (uint8_t)(duty_16b >> 8);
      OCR1CL = (uint8_t)duty_16b;
      break;


    case 2:
      OCR3BH = (uint8_t)(duty_16b >> 8);
      OCR3BL = (uint8_t)duty_16b;
      break;

    case 3:
      OCR3CH = (uint8_t)(duty_16b >> 8);
      OCR3CL = (uint8_t)duty_16b;
      break;

    case 5:
      OCR3AH = (uint8_t)(duty_16b >> 8);
      OCR3AL = (uint8_t)duty_16b;
      break;

    //Timer 4
    case 6:
      OCR4AH = (uint8_t)(duty_16b >> 8);
      OCR4AL = (uint8_t)duty_16b;
      break;

    case 7:
      OCR4BH = (uint8_t)(duty_16b >> 8);
      OCR4BL = (uint8_t)duty_16b;
      break;

    case 8:
      OCR4CH = (uint8_t)(duty_16b >> 8);
      OCR4CL = (uint8_t)duty_16b;
      break;

    //Timer 2
    case 9:
      OCR2B = (uint8_t)duty_16b;
      break;

    case 10:
      OCR2A = (uint8_t)duty_16b;
      break;

    default:
      return;
  }

  if (digital_Setup[pin_PWM] == 0) {
    pinMode(pin_LPWM, OUTPUT);
    pinMode(pin_RPWM, OUTPUT);
    digital_Setup[pin_PWM]++;
  }

  if (duty == 0) {
    digitalWrite(pin_LPWM, 0);
    digitalWrite(pin_RPWM, 0);
  } else {
    digitalWrite(pin_LPWM, (duty <= 0) ? 0 : 1);
    digitalWrite(pin_RPWM, (duty <= 0) ? 1 : 0);
  }
}
#endif