#include "ch.h"
#include "hal.h"

#include "dfi.h"


#define DATA_LINES    6
#define MODULES      10
#define FET_COUNT     8
#define SEGMENTS      6

volatile uint8_t data[DATA_LINES][MODULES][FET_COUNT][SEGMENTS];
static virtual_timer_t mosfet_timer;

// prototypes
void initializeSRData(void);
static void refreshDisplay(void *arg);
void shiftOut(uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t);


THD_WORKING_AREA(wa_dfiFunc, DFI_THREAD_STACK_SIZE);
THD_FUNCTION(dfiFunc, p) {
  (void)p;
  chRegSetThreadName("dfi");

  static systime_t last_time_simul = 0;
  static uint8_t counterFet = 0;
  static uint8_t counterDigit = 0;
  static uint8_t counterBit = 0;
  chVTObjectInit(&mosfet_timer);
  chVTSet(&mosfet_timer, US2ST(1500), refreshDisplay, NULL);

  while(true) {

      if(ST2MS(chVTTimeElapsedSinceX(last_time_simul)) > 5) {

        for(int dataline = 0; dataline < DATA_LINES; dataline++) {

          data[dataline][0][0][1] = counterBit;
          data[dataline][1][0][1] = 255 - counterBit;
          data[dataline][2][0][1] = counterBit;
          data[dataline][3][0][1] = counterBit;
          data[dataline][4][0][1] = counterBit;
          data[dataline][5][0][1] = counterBit;
          data[dataline][6][0][1] = counterBit;
          data[dataline][7][0][1] = counterBit;
          data[dataline][8][0][1] = counterBit;
          data[dataline][9][0][1] = counterBit;


        }
/*
        for(int fet=0; fet<FET_COUNT; fet++) {
          for(int digit=0; digit<BYTE_PER_FET; digit++) {
            if(fet == 0 && digit == counterDigit) {
              data[fet][digit] = 0xff; // (1 << counterBit);
            } else {
              data[fet][digit] = 0xff;
            }
          }
        }

        counterBit++;
        if(counterBit > 7) {
          counterBit = 0;
          counterDigit++;

          if(counterDigit > 5) {
            counterFet++;
            counterDigit = 0;
            counterFet %= FET_COUNT;
          }
        }
*/
        counterBit++;
        last_time_simul = chVTGetSystemTimeX();
        palTogglePad(GPIOC, GPIOC_LED);
      }
  }
}


void initializeSRData() {
  palSetPad(GPIOE, GPIOE_PIN1);      //Tells all SRs that uController is sending data

  for(int seg = 0; seg < SEGMENTS; seg++) {
    shiftOut(0,0,0,0,0,0);
  }

  palClearPad(GPIOE, GPIOE_PIN1);      //Tells all SRs that uController is done sending data
}


static void refreshDisplay(void *arg) {
    (void)arg;

    chSysLockFromISR();
    static int fet = 0;

    fet++;
    fet %= FET_COUNT;

    // switch fets

    for(int module = MODULES-1; module >= 0; module--) {
      for(int seg = 0; seg < SEGMENTS; seg++) {
        shiftOut(data[0][module][fet][seg],
                 data[1][module][fet][seg],
                 data[2][module][fet][seg],
                 data[3][module][fet][seg],
                 data[4][module][fet][seg],
                 data[5][module][fet][seg]);
      }
    }

    palClearPort(GPIOD, 0xff00);  // fets off

    for (int w = 0; w < 125; w++) { // wait ca 8µS    /7 125
      asm ("nop");
    }
    palSetPad(GPIOE, GPIOE_PIN1); // latch high
    for (int w = 0; w < 25; w++) {   // wait ca 1,6µS = 25
      asm ("nop");
    }

    palClearPad(GPIOE, GPIOE_PIN1);  // latch low


    for (int w = 0; w < 13; w++) { // wait ca 1µS
      asm ("nop");
    }

    // switch fets
    //palSetPort(GPIOD, (1 << (fet+8)));
    palWritePad(GPIOD, GPIOD_PIN8, (fet == 0) ? 1 : 0);
    palWritePad(GPIOD, GPIOD_PIN9, (fet == 1) ? 1 : 0);
    palWritePad(GPIOD, GPIOD_PIN10, (fet == 2) ? 1 : 0);
    palWritePad(GPIOD, GPIOD_PIN11, (fet == 3) ? 1 : 0);
    palWritePad(GPIOD, GPIOD_PIN12, (fet == 4) ? 1 : 0);
    palWritePad(GPIOD, GPIOD_PIN13, (fet == 5) ? 1 : 0);
    palWritePad(GPIOD, GPIOD_PIN14, (fet == 6) ? 1 : 0);
    palWritePad(GPIOD, GPIOD_PIN15, (fet == 7) ? 1 : 0);


    //palClearPort(GPIOD, 0xff00);

    chVTSetI(&mosfet_timer, US2ST(1500), refreshDisplay, NULL);
    chSysUnlockFromISR();

}




void shiftOut(uint8_t val1, uint8_t val2, uint8_t val3,
              uint8_t val4, uint8_t val5, uint8_t val6) {

    uint8_t i;

    for (i = 0; i < 8; i++) {

        palWritePad(GPIOE, GPIOE_PIN2, (val1 & (1 << i)) >> i);
        palWritePad(GPIOE, GPIOE_PIN3, (val2 & (1 << i)) >> i);
        palWritePad(GPIOE, GPIOE_PIN4, (val3 & (1 << i)) >> i);
        palWritePad(GPIOE, GPIOE_PIN5, (val4 & (1 << i)) >> i);
        palWritePad(GPIOE, GPIOE_PIN6, (val5 & (1 << i)) >> i);
        palWritePad(GPIOE, GPIOE_PIN7, (val6 & (1 << i)) >> i);

/*        int val = ((val6 & (1 << i) >> i) << 7) |
                  ((val5 & (1 << i) >> i) << 6) |
                  ((val4 & (1 << i) >> i) << 5) |
                  ((val3 & (1 << i) >> i) << 4) |
                  ((val2 & (1 << i) >> i) << 3) |
                  ((val1 & (1 << i) >> i) << 2);

        palWriteGroup(GPIOE, 0b11111100, 0, val);
*/
        palSetPad(GPIOE, GPIOE_PIN0);     //clock
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        palClearPad(GPIOE, GPIOE_PIN0);   //clock

    }
}



void init_hw() {

  initializeSRData();

  palSetGroupMode(GPIOD, 0xff00, 0, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
  palSetGroupMode(GPIOE, 0x00ff, 0, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);


  /*
    // enable clock
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_GPIOE, ENABLE);

    GPIO_InitTypeDef GPIO_InitStructure;

    GPIO_InitStructure.GPIO_Pin = GPIOD_PIN8 | GPIOD_PIN9 | GPIOD_PIN10 |
        GPIOD_PIN11 | GPIOD_PIN12 | GPIOD_PIN13 | GPIOD_PIN14 | GPIOD_PIN15;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(GPIOD, &GPIO_InitStructure);


    GPIO_InitStructure.GPIO_Pin = GPIOE_PIN0 | GPIOE_PIN1 | GPIOE_PIN2 |
        GPIOE_PIN3 | GPIOE_PIN4 | GPIOE_PIN5 | GPIOE_PIN6 | GPIOE_PIN7;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(GPIOE, &GPIO_InitStructure);

*/

}