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

#include "dfi.h"


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

//volatile uint8_t data[DATA_LINES][MODULES][FET_COUNT][SEGMENTS];
volatile uint8_t data[FET_COUNT][SEGMENTS * MODULES * BIT_PER_FET_PER_SEG];
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 counterBit = 0;

  chVTObjectInit(&mosfet_timer);
  chVTSet(&mosfet_timer, US2ST(1570), refreshDisplay, NULL);

  while(true) {

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

        for(int fet = 0; fet< FET_COUNT; fet++) {
          for (size_t i = 0; i < 480; i++) {
            data[fet][i] = 0xff;
          }
        }
/*
        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 (uint16_t i = 0; i < SEGMENTS * MODULES * BIT_PER_FET_PER_SEG; i++) {
      palWriteGroup(GPIOE, 0b11111100, 0, 0);
      palSetPad(GPIOE, GPIOE_PIN0);     //clock
      asm ("nop");
      asm ("nop");
      asm ("nop");
      asm ("nop");
      palClearPad(GPIOE, GPIOE_PIN0);   //clock
      asm ("nop");
      asm ("nop");
  }

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


// all 1.5ms
static void refreshDisplay(void *arg) {
    (void)arg;

    chSysLockFromISR();
    static int fet = 0;

    fet++;
    fet %= FET_COUNT;


    for (uint16_t i = 0; i < SEGMENTS * MODULES * BIT_PER_FET_PER_SEG; i++) {
        palClearPort(GPIOE, 0xfc);
        palWriteGroup(GPIOE, 0xfc, 0, data[fet][i]);

        //palSetPort(GPIOE, data[fet][i]);

        palSetPad(GPIOE, GPIOE_PIN0);     //clock
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        asm ("nop");
        palClearPad(GPIOE, GPIOE_PIN0);   //clock
    }



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

    for (uint8_t w = 0; w < 125; w++) {
      asm ("nop");
    }

    palSetPad(GPIOE, GPIOE_PIN1); // latch high
    for (uint8_t w = 0; w < 60; w++) {
      asm ("nop");
    }
    palClearPad(GPIOE, GPIOE_PIN1);  // latch low


    for (uint8_t w = 0; w < 26; w++) {
      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);


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

}







void init_hw() {

  initializeSRData();

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


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

*/

}