// C.24/C.25/C.26 = D6/D5/D4
Pio *p = digitalPinToPort(4);
uint32_t b4 = digitalPinToBitMask(4);
uint32_t b5 = digitalPinToBitMask(5);
uint32_t b6 = digitalPinToBitMask(6);

// Enable synchronous, 14-bit resolution PWM on 3 channels
void setup() {
  // PWM set-up on pins D9, D8 and D7 
  REG_PMC_PCER1 |= PMC_PCER1_PID36;                              // Enable PWM 

  REG_PIOC_ABSR |= PIO_ABSR_P23 | PIO_ABSR_P22 | PIO_ABSR_P21;   // Set the port C PWM pins to peripheral type B
  REG_PIOC_PDR  |= PIO_PDR_P23  | PIO_PDR_P22  | PIO_PDR_P21;    // Set the port C PWM pins to outputs
  
  REG_PWM_CLK = PWM_CLK_PREA(0) | PWM_CLK_DIVA(1);               // Set the PWM clock A rate to 84MHz (84MHz/1)
  
  REG_PWM_SCM |= PWM_SCM_SYNC6 | PWM_SCM_SYNC5 | PWM_SCM_SYNC4;  // Set the PWM channels as synchronous

  PWM->PWM_CH_NUM[0].PWM_CMR = PWM_CMR_CPRE_CLKA;                // Enable centre aligned PWM and set the clock source as CLKA
  PWM->PWM_CH_NUM[0].PWM_CPRD = 21000;                           // Set the PWM period register 84MHz/(2*2kHz)=21000;

  PWM->PWM_CH_NUM[1].PWM_CMR = PWM_CMR_CPRE_CLKA;                // Enable centre aligned PWM and set the clock source as CLKA
  PWM->PWM_CH_NUM[1].PWM_CPRD = 21000;                           // Set the PWM period register 84MHz/(2*2kHz)=21000;

  PWM->PWM_CH_NUM[2].PWM_CMR = PWM_CMR_CPRE_CLKA;                // Enable centre aligned PWM and set the clock source as CLKA
  PWM->PWM_CH_NUM[2].PWM_CPRD = 21000;                           // Set the PWM period register 84MHz/(2*2kHz)=21000;

  REG_PWM_ENA = PWM_ENA_CHID0;                                   // Enable the PWM channels, (only need to set channel 0 for synchronous mode)

  PWM->PWM_CH_NUM[0].PWM_CDTYUPD = 0;                            // ?!?!?
  PWM->PWM_CH_NUM[4].PWM_CDTYUPD = 12600;                        // Set the PWM duty cycle to 60%            (21000*0.6=12600)
  PWM->PWM_CH_NUM[5].PWM_CDTYUPD = 7000;                         // Set the PWM duty cycle to 120°=33.33%    (21000/3=7000)
  PWM->PWM_CH_NUM[6].PWM_CDTYUPD = 19600;                        // Set the PWM duty cycle to 33%+60%=93.33% (12600+7000=19600)

  REG_PWM_SCUC = PWM_SCUC_UPDULOCK;      // Set the update unlock bit to trigger an update at the end of the next PWM period

  pinMode(4, OUTPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
}

void loop() {
  for(;;) {
    uint32_t v;
    v = p->PIO_PDSR;
    
    if (bool(v & b6) ^ bool(v & b5)) {
      p->PIO_SODR = b4;
    } else {
      p->PIO_CODR = b4;
    }
  }
}