// Procédures utiles pour la carte MSP-EXP430F5529
// Pierre-Yves Rochat, 2020, EPFL, pyr@pyr.ch

#include <Carte2026.h>

void InitCarte2025() {
  InitLed1;
  InitLed2;
  InitLed3;
  InitLed4;
  InitLed5;
  InitLed6;
  InitLed7;
  InitLed8;

  InitPous1;
  InitPous2;
  InitPous3;
  InitPous4;
  InitPous5;

  InitEncodX;
  InitEncodY;
  InitPousEncod;
}

void AfficheLedBleues(uint16_t val) {
  if (val & (1<<0)) { Led8On; } else { Led8Off; }
  if (val & (1<<1)) { Led7On; } else { Led7Off; }
  if (val & (1<<2)) { Led6On; } else { Led6Off; }
  if (val & (1<<3)) { Led5On; } else { Led5Off; }
  if (val & (1<<4)) { Led4On; } else { Led4Off; }
}


#define CPU_F ((double)25000000)
#define Delay_ms(x) __delay_cycles((long)(CPU_F*(double)x/1000.0))

void delay_ms(unsigned int ms) {
    while(ms) {
        Delay_ms(1);
        ms--;
    }
}

// Procédures pour passer la fréquence de 25 MHz
// (fournies par Texas Instrument !)
void SetVCoreUp (unsigned int level) {
  PMMCTL0_H = 0xA5; // Open PMM registers for write access
  // Set SVS/SVM high side new level :
  SVSMHCTL = SVSHE + SVSHRVL0 * level + SVMHE + SVSMHRRL0 * level;
  // Set SVM low side to new level :
  SVSMLCTL = SVSLE + SVMLE + SVSMLRRL0 * level;
  while ((PMMIFG & SVSMLDLYIFG) == 0) {} // Wait till SVM is settled
  PMMIFG &= ~(SVMLVLRIFG + SVMLIFG); // Clear already set flags
  PMMCTL0_L = PMMCOREV0 * level; // Set VCore to new level
  if ((PMMIFG & SVMLIFG)) { // Wait till new level reached
    while ((PMMIFG & SVMLVLRIFG) == 0);
  }
  // Set SVS/SVM low side to new level :
  SVSMLCTL = SVSLE + SVSLRVL0 * level + SVMLE + SVSMLRRL0 * level;
  PMMCTL0_H = 0x00; // Lock PMM registers for write access
}

void setupDCO(void) {
  SetVCoreUp(1u); // Power settings
  SetVCoreUp(2u); SetVCoreUp(3u);
  UCSCTL3 = SELREF__REFOCLK; // select REFO as FLL source
  UCSCTL6 = XT1OFF | XT2OFF; // turn off XT1 and XT2
  // Initialize DCO to 25.00MHz :
  __bis_SR_register(SCG0); // Disable the FLL control loop
  UCSCTL0 = 0x0000u;  // Set lowest possible DCOx, MODx
  UCSCTL1 = DCORSEL_6; // Set RSELx for DCO = 50 MHz
  UCSCTL2 = 762u;  // Set DCO Multiplier for 25MHz
                       // (N + 1) * FLLRef = Fdco, (762 + 1) * 32768 = 25.00MHz
  UCSCTL4 = SELA__REFOCLK | SELS__DCOCLK | SELM__DCOCLK;
  __bic_SR_register(SCG0); // Enable the FLL control loop

  // Worst-case settling time for the DCO when the DCO range bits have been
  // changed is n x 32 x 32 x f_MCLK / f_FLL_reference. See UCS chapter in 5xx
  // UG for optimization.
  // 32*32*25MHz/32768Hz = 781250 = MCLK cycles for DCO to settle
  __delay_cycles(781250u);

  do { // Loop until XT1,XT2 & DCO fault flag is cleared
    UCSCTL7 &= ~(XT2OFFG + XT1LFOFFG + DCOFFG); // Clear XT2,XT1,DCO fault flags
    SFRIFG1 &= ~OFIFG; // Clear fault flags
  } while (SFRIFG1&OFIFG); // Test oscillator fault flag
}

void InitMoteur() {
  P7DIR |= (1<<0); // EN
  P7DIR |= (1<<4); // DIR

  P1DIR &=~(1<<2); P1REN |= (1<<2); P1OUT |= (1<<2); // encodeur X
  P1DIR &=~(1<<3); P1REN |= (1<<3); P1OUT |= (1<<3); // encodeur Y
  P1DIR &=~(1<<4); P1REN |= (1<<4); P1OUT |= (1<<4); // fin course gauche
  P1DIR &=~(1<<5); P1REN |= (1<<5); P1OUT |= (1<<5); // fin course droite
}