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

#include "dfi.h"


#define FET_COUNT     8
#define BYTE_PER_FET  6

volatile uint8_t data[FET_COUNT][BYTE_PER_FET];
static virtual_timer_t mosfet_timer;

// prototypes
void initializeSRData(void);
static void refreshDisplay(void *arg);
void shiftOut(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, MS2ST(10), refreshDisplay, NULL);

  while(true) {

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

        for(int fet=0; fet<FET_COUNT; fet++) {
          for(int digit=0; digit<BYTE_PER_FET; digit++) {
            if(fet == counterFet && 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;
          }
        }

        last_time_simul = chVTGetSystemTimeX();
        palTogglePad(GPIOC, GPIOC_LED);
      }
  }
}


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

  for(int digit = 0; digit < BYTE_PER_FET; digit++) {
    shiftOut(0);
  }

  for(int fet=0; fet<FET_COUNT; fet++) {
    for(int digit=0; digit<BYTE_PER_FET; digit++) {
      data[fet][digit] = 0xff;
    }
  }

  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 digit = 0; digit < BYTE_PER_FET; digit++) {
      shiftOut(data[fet][digit]);
    }

    palClearPort(GPIOD, 0xff00);

    palSetPad(GPIOE, GPIOE_PIN1); // latch
    for (int w = 0; w < 1000; w++);
    palClearPad(GPIOE, GPIOE_PIN1);  // latch



    // 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, MS2ST(2), refreshDisplay, NULL);
    chSysUnlockFromISR();

}




void shiftOut(uint8_t val) {
    uint8_t i;

    uint8_t bitbit = false;
    for (i = 0; i < 8; i++)  {
        //bitbit = !!((val & (1 << i)));
        bitbit = (val & (1 << i)) >> i;
        /*
        if(bitbit == 1) {
            palSetPad(GPIOE, GPIOE_PIN2);
        } else {
            palClearPad(GPIOE, GPIOE_PIN2);
        }*/
        palWritePad(GPIOE, GPIOE_PIN2, bitbit);

        palSetPad(GPIOE, GPIOE_PIN0);     //clock
        for (int w = 0; w < 1000; w++);
        palClearPad(GPIOE, GPIOE_PIN0);   //clock
        for (int w = 0; w < 1000; w++);
    }
}



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);

*/

}