#include <stdint.h>
#include <mem_io.h>
#include <scmt.h>
#include <scmt_config.h>
#include <scmt_register.h>

#if SCMT_DEBUG
  #include <log.h>
#endif /* SCMT_DEBUG */

#if SCMT_TOGGLE_GPIO
  #include <pfc.h>
void gpio_set(void)
{
    /* V4H: SCL3_V::Pin AK5::GP8_06
       GPIO Group 8 Base: H’E606 8000
       INOUTSELn +0x184 (set pin to output mode)
       OUTDTn +0x188 (set pin output value)
       */
    pfc_reg_write(PFC_INOUTSEL8_RW, 1<<6); /* Set GP8_06 to output mode */
    pfc_reg_write(PFC_OUTDT8_RW, 1<<6);    /* Set GP8_06 to 'high' */
}
static int gpio_reset_cnt = 3;
void gpio_clear(void)
{
    if (gpio_reset_cnt>0) {
        gpio_reset_cnt--;
    }
    else
    {
        pfc_reg_write(PFC_OUTDT8_RW, 0);    /* Set GP8_06 to 'low' */
    }

}
#endif /* SCMT_TOGGLE_GPIO */

void scmt_module_start(void)
{
    /* For boot time measurement, signal start of SCMT by GPIO pin toggle */
    #if SCMT_TOGGLE_GPIO
    gpio_set();
    #endif /* SCMT_TOGGLE_GPIO */

    /* If you have issues with the code getting stuck or reading zero from the timer,
       you can debug the init seqence with SCMT_DEBUG.
       Just make sure that printing debug data is already possible. */
  #if SCMT_DEBUG
    PRINTFN("SCMT A\n");
  #endif /* SCMT_DEBUG */
  #if SCMT_INIT
    mem_write32(SCMT_CMSCNT,(SCMT_START_VALUE)); 	/* Set counter value to zero */
  #endif /* SCMT_INIT */
  #if SCMT_DEBUG
    PRINTFN("SCMT B: SCMT_CMSCNT (0x%x) = 0x%x\n", SCMT_CMSCNT, mem_read32(SCMT_CMSCNT) );
  #endif /* SCMT_DEBUG */
  #if SCMT_INIT
    mem_write32(SCMT_CMSCOR,0xffffffff);    		/* Set compare value to maximum, we use it as 32-bit counter only */
  #endif /* SCMT_INIT */
  #if SCMT_DEBUG
    PRINTFN("SCMT C: SCMT_CMSCOR (0x%x) = 0x%x\n", SCMT_CMSCOR, mem_read32(SCMT_CMSCOR) );
  #endif /* SCMT_DEBUG */
  #if SCMT_INIT
    while(mem_read16(SCMT_CMSCSR)&(1<<13)); 		/* Wait for write clearance */
  #endif /* SCMT_INIT */
  #if SCMT_DEBUG
    PRINTFN("SCMT D: SCMT_CMSCSR (0x%x) = 0x%x\n", SCMT_CMSCSR, mem_read16(SCMT_CMSCSR) );
  #endif /* SCMT_DEBUG */
  #if SCMT_INIT
    mem_write16(SCMT_CMSSTR,1<<5);          		/* Start counter */
  #endif /* SCMT_INIT */
  #if SCMT_DEBUG
    PRINTFN("SCMT E: SCMT_CMSSTR (0x%x) = 0x%x\n", SCMT_CMSSTR, mem_read16(SCMT_CMSSTR) );
    PRINTFN("SCMT F: SCMT_CMSCNT (0x%x) = 0x%x\n", SCMT_CMSCNT, mem_read32(SCMT_CMSCNT) );
    PRINTFN("SCMT G: SCMT_CMSCNT (0x%x) = 0x%x\n", SCMT_CMSCNT, mem_read32(SCMT_CMSCNT) );
    PRINTFN("SCMT H: SCMT_CMSCNT (0x%x) = 0x%x\n", SCMT_CMSCNT, mem_read32(SCMT_CMSCNT) );
    PRINTFN("SCMT I: SCMT_CMSCNT (0x%x) = 0x%x\n", SCMT_CMSCNT, mem_read32(SCMT_CMSCNT) );
  #endif /* SCMT_DEBUG */
}

uint32_t scmt_module_read(void)
{
    uint32_t last = 0, current = 0;

    /* For boot time measurement, signal start of SCMT by GPIO pin toggle, reset after some time */
    #if SCMT_TOGGLE_GPIO
    gpio_clear();
    #endif /* SCMT_TOGGLE_GPIO */

    /* From UM:
       When CMSCNT is read during the counter operation, the read value may be wrong because of an asynchronous clock between counter and bus-interface.
       For exact value, read this register continuously, until same values are read from this register. */
    current = mem_read32(SCMT_CMSCNT);
    do {
        last = current;
        current = mem_read32(SCMT_CMSCNT);
    } while (last != current);
    return current;
}

/* NOT SAFE FOR OVERLFOWS */
void scmt_wait_ticks(uint32_t ticks)
{
	uint32_t start = scmt_module_read();
	uint32_t stop = start + ticks;

	while (stop > scmt_module_read()) { /* NOP */ };
}