#include <stm32h747xx.h>
#include "display.h"
#include "registers.h"
#include "delay.h"

void display_init(display_t *display, DSI_TypeDef *dsi, LTDC_TypeDef* ltdc, LTDC_Layer_TypeDef* layer, display_config_t display_config) {
  display->dsi = dsi;
  display->ltdc = ltdc;
  display->config = display_config;
  display->layer = layer;
}

void display_setup(display_t *display, uint32_t dsi_khz, uint32_t ltdc_khz,
                   dsi_mode_t mode,
                   dsi_pll_config_t pll, dsi_phy_config_t phy,
                   dsi_flow_config_t flow, dsi_config_t dsi_config) {

  // TODO: interrupts?
  RCC->APB3ENR |= RCC_APB3ENR_DSIEN | RCC_APB3ENR_LTDCEN;

  // Configure the RCC
  RCC->APB3RSTR |= RCC_APB3RSTR_LTDCRST | RCC_APB3RSTR_DSIRST;
  delay_us(3);
  RCC->APB3RSTR &= ~(RCC_APB3RSTR_LTDCRST | RCC_APB3RSTR_DSIRST);

  RCC->APB3ENR |= RCC_APB3ENR_DSIEN | RCC_APB3ENR_LTDCEN;

  // Turn on DSI regulator, wait for ready
  reg_set_bits(&display->dsi->WRPCR, DSI_WRPCR_REGEN);
  while (!(display->dsi->WISR & DSI_WISR_RRIF));

  // Confiugre DSI PLL, turn it ON and wait for lock
  reg_write_bits(&display->dsi->WRPCR, (pll.divn << DSI_WRPCR_PLL_NDIV_Pos) |
                 (pll.idf << DSI_WRPCR_PLL_IDF_Pos) |
                 (pll.odf << DSI_WRPCR_PLL_ODF_Pos),
                 DSI_WRPCR_PLL_NDIV_Msk | DSI_WRPCR_PLL_IDF_Msk | DSI_WRPCR_PLL_ODF_Msk);

  reg_set_bits(&display->dsi->WRPCR, DSI_WRPCR_PLLEN);
  delay_us(400);
  while (!(display->dsi->WISR & DSI_WISR_PLLLIF));

  // Enable D-PHY
  reg_set_bits(&display->dsi->PCTLR, DSI_PCTLR_DEN);

  // Enable D-PHY clock lane
  reg_set_bits(&display->dsi->PCTLR, DSI_PCTLR_CKE);

  // Configure LTDC
  reg_write_bits(&display->ltdc->GCR,
                 (display->config.h_sync_pol << LTDC_GCR_HSPOL_Pos) |
                     (display->config.v_sync_pol << LTDC_GCR_VSPOL_Pos) |
                 (display->config.not_data_enable_pol << LTDC_GCR_DEPOL_Pos) | (display->config.pixel_clock_pol << LTDC_GCR_PCPOL_Pos),
                 LTDC_GCR_HSPOL_Msk | LTDC_GCR_VSPOL_Msk | LTDC_GCR_DEPOL_Msk | LTDC_GCR_PCPOL_Msk);

  reg_write_bits(&display->ltdc->SSCR,
                 ((display->config.v_sync - 1) << LTDC_SSCR_VSH_Pos) |
                 ((display->config.h_sync - 1) << LTDC_SSCR_HSW_Pos),
                 LTDC_SSCR_VSH_Msk | LTDC_SSCR_HSW_Msk);

  reg_write_bits(&display->ltdc->BPCR,
                 ((display->config.v_sync + display->config.v_back_porch - 1) << LTDC_BPCR_AVBP_Pos) |
                 ((display->config.h_sync + display->config.h_back_porch - 1) << LTDC_BPCR_AHBP_Pos),
                 LTDC_BPCR_AVBP_Msk | LTDC_BPCR_AHBP_Msk);

  reg_write_bits(&display->ltdc->AWCR,
                 ((display->config.v_sync + display->config.v_back_porch +
                   display->config.active_height - 1) << LTDC_AWCR_AAH_Pos) |
                 ((display->config.active_width + display->config.h_sync +
                   display->config.h_back_porch - 1) << LTDC_AWCR_AAW_Pos),
                 LTDC_AWCR_AAH_Msk | LTDC_AWCR_AAW_Msk);

  reg_write_bits(&display->ltdc->TWCR,
                 ((display->config.active_height +
                   display->config.v_front_porch +
                   display->config.v_back_porch - 1)
                  << LTDC_TWCR_TOTALH_Pos) |
                 ((display->config.active_width +
                   display->config.h_front_porch +
                   display->config.h_back_porch - 1)
                  << LTDC_TWCR_TOTALW_Pos),
                 LTDC_TWCR_TOTALH_Msk | LTDC_TWCR_TOTALW_Msk);

  reg_write_bits(&display->ltdc->BCCR, 0, LTDC_BCCR_BCBLUE_Msk | LTDC_BCCR_BCRED_Msk | LTDC_BCCR_BCGREEN_Msk);

  // Enable LTDC in the LTDC
  reg_set_bits(&display->ltdc->GCR, LTDC_GCR_LTDCEN);

  // Configure D-PHY parameters in dsi host and wrapper
  reg_write_bits(&display->dsi->WPCR[0], phy.unit_interval << DSI_WPCR0_UIX4_Pos, DSI_WPCR0_UIX4_Msk);
  reg_write_bits(&display->dsi->PCONFR, phy.data_lanes << DSI_PCONFR_NL_Pos, DSI_PCONFR_NL_Msk);
  reg_write_bits(&display->dsi->CLCR,
                 (phy.auto_clock_lane_control << DSI_CLCR_ACR_Pos) |
                     (phy.clock_control << DSI_CLCR_DPCC_Pos),
                 DSI_CLCR_ACR_Msk | DSI_CLCR_DPCC_Msk);

  uint32_t max_delay = (phy.clock_hs2lp > phy.clock_lp2hs) ? phy.clock_hs2lp : phy.clock_lp2hs;
  reg_write_bits(&display->dsi->CLTCR,
                 (max_delay << DSI_CLTCR_HS2LP_TIME_Pos) |
                 (max_delay << DSI_CLTCR_LP2HS_TIME_Pos),
                 DSI_CLTCR_HS2LP_TIME_Msk | DSI_CLTCR_LP2HS_TIME_Msk);
  reg_write_bits(&display->dsi->DLTCR,
                 (phy.data_hs2lp << DSI_DLTCR_HS2LP_TIME_Pos) |
                     (phy.data_lp2hs << DSI_DLTCR_LP2HS_TIME_Pos) |
                 (phy.max_read_time << DSI_DLTCR_MRD_TIME_Pos),
                 DSI_DLTCR_HS2LP_TIME_Msk | DSI_DLTCR_LP2HS_TIME_Msk | DSI_DLTCR_MRD_TIME_Msk);

  reg_write_bits(&display->dsi->PCONFR,
                 phy.stop_wait_time << DSI_PCONFR_SW_TIME_Pos,
                 DSI_PCONFR_SW_TIME_Msk);

  // Configure DSI host timing
  reg_write_bits(&display->dsi->CCR,
                 pll.eckdiv << DSI_CCR_TXECKDIV_Pos,
                 DSI_CCR_TXECKDIV_Msk);
  // TODO: are timeouts required to be configured?
  /* reg_write_bits(&display->dsi->TCCR[0], );*/

  // Configure DSI host flow control and dbi interface
  reg_write_bits(&display->dsi->PCR,
                 (flow.bta_enable << DSI_PCR_BTAE_Pos) |
                 (flow.ecc_reception_enable << DSI_PCR_ECCRXE_Pos) |
                 (flow.crc_reception_enable << DSI_PCR_CRCRXE_Pos) |
                 (flow.eotp_reception_enable << DSI_PCR_ETRXE_Pos) |
                 (flow.eotp_transmission_enable << DSI_PCR_ETTXE_Pos),
                 DSI_PCR_BTAE_Msk | DSI_PCR_ECCRXE_Msk | DSI_PCR_ECCRXE_Msk | DSI_PCR_ETRXE_Msk | DSI_PCR_ETTXE_Msk);

  // Configure DSI host for video mode
  if (mode == DSI_MODE_VIDEO) {
    reg_clear_bits(&display->dsi->MCR, DSI_MCR_CMDM);
    reg_write_bits(&display->dsi->WCFGR, (0 << DSI_WCFGR_DSIM_Pos) |
                   (0 << DSI_WCFGR_TESRC_Pos) |
                   (0 << DSI_WCFGR_TEPOL_Pos) | (0 << DSI_WCFGR_AR_Pos),
                   DSI_WCFGR_DSIM_Msk | DSI_WCFGR_TESRC_Msk | DSI_WCFGR_TEPOL_Msk | DSI_WCFGR_AR_Msk);

    reg_write_bits(&display->dsi->VMCR,
                   (display->config.video_mode << DSI_VMCR_VMT_Pos) |
                       (1 << DSI_VMCR_LPVSAE_Pos) | (1 << DSI_VMCR_LPVBPE_Pos) |
                       (1 << DSI_VMCR_LPVFPE_Pos) | (1 << DSI_VMCR_LPVAE_Pos) |
                       (1 << DSI_VMCR_LPHBPE_Pos) | (1 << DSI_VMCR_LPHFPE_Pos) |
                       (1 << DSI_VMCR_LPCE_Pos) | (1 << DSI_VMCR_FBTAAE_Pos),
                   DSI_VMCR_VMT_Msk | DSI_VMCR_LPVSAE_Msk |
                   DSI_VMCR_LPVBPE_Msk | DSI_VMCR_LPVFPE_Msk |
                   DSI_VMCR_LPVAE_Msk | DSI_VMCR_LPHBPE_Msk |
                   DSI_VMCR_LPHFPE_Msk |
                   DSI_VMCR_LPCE_Msk | DSI_VMCR_FBTAAE_Msk);

    reg_write_bits(&display->dsi->VPCR,
                   display->config.active_width << DSI_VPCR_VPSIZE_Pos,
                   DSI_VPCR_VPSIZE_Msk);

    reg_write_bits(&display->dsi->VCCR, 1 << DSI_VCCR_NUMC_Pos, DSI_VCCR_NUMC_Msk);
    reg_write_bits(&display->dsi->VNPCR, 0 << DSI_VNPCR_NPSIZE_Pos, DSI_VNPCR_NPSIZE_Msk);

    // Horizontal sync active
    uint32_t hsa = display->config.h_sync * dsi_khz / ltdc_khz;
    reg_write_bits(&display->dsi->VHSACR, (hsa << DSI_VHSACR_HSA_Pos), DSI_VHSACR_HSA_Msk);
    // Horizontal back porch
    uint32_t hbp = display->config.h_back_porch * dsi_khz / ltdc_khz;
    reg_write_bits(&display->dsi->VHBPCR, (hbp << DSI_VHBPCR_HBP_Pos), DSI_VHBPCR_HBP_Msk);

    uint32_t hline = display->config.h_sync + display->config.h_back_porch + display->config.active_width + display->config.h_front_porch;
    hline *= dsi_khz;
    hline /= ltdc_khz;
    reg_write_bits(&display->dsi->VLCR, (hline << DSI_VLCR_HLINE_Pos), DSI_VLCR_HLINE_Msk);

    reg_write_bits(&display->dsi->VVSACR, display->config.v_sync << DSI_VVSACR_VSA_Pos, DSI_VVSACR_VSA_Msk);
    reg_write_bits(&display->dsi->VVBPCR, display->config.v_back_porch << DSI_VVBPCR_VBP_Pos, DSI_VVBPCR_VBP_Msk);
    reg_write_bits(&display->dsi->VVFPCR, display->config.v_front_porch << DSI_VVFPCR_VFP_Pos, DSI_VVFPCR_VFP_Msk);
    reg_write_bits(&display->dsi->VVACR, display->config.active_height << DSI_VVACR_VA_Pos, DSI_VVACR_VA_Msk);
  } else {
    reg_set_bits(&display->dsi->MCR, DSI_MCR_CMDM);

    reg_write_bits(&display->dsi->WCFGR, (1 << DSI_WCFGR_DSIM_Pos) |
                   (0 << DSI_WCFGR_TESRC_Pos) |
                   (0 << DSI_WCFGR_TEPOL_Pos) | (0 << DSI_WCFGR_AR_Pos) | (0 << DSI_WCFGR_VSPOL_Pos),
                   DSI_WCFGR_DSIM_Msk | DSI_WCFGR_TESRC_Msk | DSI_WCFGR_TEPOL_Msk | DSI_WCFGR_AR_Msk | DSI_WCFGR_VSPOL_Msk);

    reg_write_bits(&display->dsi->LCCR, display->config.active_width << DSI_LCCR_CMDSIZE_Pos, DSI_LCCR_CMDSIZE_Msk);
    reg_set_bits(&display->dsi->CMCR, DSI_CMCR_TEARE);
  }

  // Configure DSI host LTDC
  reg_write_bits(&display->dsi->LVCIDR,
                 dsi_config.channel << DSI_LVCIDR_VCID_Pos,
                 DSI_LVCIDR_VCID_Msk);

  reg_write_bits(&display->dsi->LPCR,
                 (0 << DSI_LPCR_DEP_Pos) |
                 (0 << DSI_LPCR_VSP_Pos) |
                 (0 << DSI_LPCR_HSP_Pos),
                 DSI_LPCR_DEP_Msk | DSI_LPCR_VSP_Msk | DSI_LPCR_HSP_Msk);

  reg_write_bits(&display->dsi->LCOLCR,
                 (dsi_config.color_coding << DSI_LCOLCR_COLC_Pos) |
                 (dsi_config.color_loosely_packed << DSI_LCOLCR_LPE_Pos) ,
                 DSI_LCOLCR_LPE_Msk | DSI_LCOLCR_COLC_Msk);


  reg_write_bits(&display->dsi->WCFGR,
                 (dsi_config.wrapper_color_coding << DSI_WCFGR_COLMUX_Pos),
                 DSI_WCFGR_COLMUX_Msk);

  reg_write_bits(&display->dsi->LPMCR,
                 (dsi_config.lp_size << DSI_LPMCR_LPSIZE_Pos) | (dsi_config.vlp_size << DSI_LPMCR_VLPSIZE_Pos),
                 DSI_LPMCR_LPSIZE_Msk | DSI_LPMCR_VLPSIZE_Msk);

  // TODO is this required?
  // Send DCS commands to configure the display?

  // Start the LTDC flow through DSI wrapper
    // display_refresh does that
}

void display_start(display_t* display)
{
  // Enable DSI host
  reg_set_bits(&display->dsi->CR, DSI_CR_EN);

  // Enable DSI wrapper
  reg_set_bits(&display->dsi->WCR, DSI_WCR_DSIEN);
}

void display_layer_setup(display_t *display, void* buffer, ltdc_layer_pixel_format_t pixel_format) {
  uint16_t h_win_start = 0 +
    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AHBP_Pos, LTDC_BPCR_AHBP_Msk >> LTDC_BPCR_AHBP_Pos) + 1;
  uint16_t h_win_stop = display->config.active_width +
    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AHBP_Pos, LTDC_BPCR_AHBP_Msk >> LTDC_BPCR_AHBP_Pos);

  reg_write_bits(&display->layer->WHPCR, (h_win_start << LTDC_LxWHPCR_WHSTPOS_Pos) |
                 (h_win_stop << LTDC_LxWHPCR_WHSPPOS_Pos)
                 , LTDC_LxWHPCR_WHSPPOS_Msk | LTDC_LxWHPCR_WHSTPOS_Msk);

  uint16_t v_win_start = 0 +
    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AVBP_Pos, LTDC_BPCR_AVBP_Msk >> LTDC_BPCR_AVBP_Pos) + 1;
  uint16_t v_win_stop = display->config.active_height +
    reg_read_bits_pos(&display->ltdc->BPCR, LTDC_BPCR_AVBP_Pos, LTDC_BPCR_AVBP_Msk >> LTDC_BPCR_AVBP_Pos);

  reg_write_bits(&display->layer->WVPCR,
                 (v_win_start << LTDC_LxWVPCR_WVSTPOS_Pos) |
                 (v_win_stop << LTDC_LxWVPCR_WVSPPOS_Pos),
                 LTDC_LxWVPCR_WVSPPOS_Msk | LTDC_LxWVPCR_WVSTPOS_Msk);

  reg_write_bits(&display->layer->PFCR, pixel_format << LTDC_LxPFCR_PF_Pos, LTDC_LxPFCR_PF_Msk);

  reg_clear_bits(&display->layer->DCCR,
                 LTDC_LxDCCR_DCALPHA_Msk | LTDC_LxDCCR_DCBLUE_Msk | LTDC_LxDCCR_DCGREEN_Msk | LTDC_LxDCCR_DCRED_Msk);
  reg_write_bits(&display->layer->CACR,
                 0xFF << LTDC_LxCACR_CONSTA_Pos, LTDC_LxCACR_CONSTA_Msk);
  reg_write_bits(
                 &display->layer->BFCR,
                 (0b110 << LTDC_LxBFCR_BF1_Pos) | (0b111 << LTDC_LxBFCR_BF2_Pos),
                 LTDC_LxBFCR_BF2_Msk | LTDC_LxBFCR_BF1_Msk);

  uint32_t width = display->config.active_width;
  uint32_t height = display->config.active_height;

  uint8_t bytes_per_pixel;
  switch (pixel_format) {
  case LTDC_ARGB8888:
    bytes_per_pixel = 4;
    break;
  case LTDC_RGB888:
    bytes_per_pixel = 3;
    break;
  case LTDC_ARGB1555:
  case LTDC_ARGB4444:
  case LTDC_RGB565:
  case LTDC_AL88:
    bytes_per_pixel = 2;
    break;
  default:
    bytes_per_pixel = 1;
    break;
  }

  reg_write_bits(&display->layer->CFBLR,
                 (width * bytes_per_pixel) << LTDC_LxCFBLR_CFBP_Pos |
                 (width * bytes_per_pixel + 7) << LTDC_LxCFBLR_CFBLL_Pos,
                 LTDC_LxCFBLR_CFBP_Msk | LTDC_LxCFBLR_CFBLL_Msk);

  reg_write_bits(&display->layer->CFBLNR, height << LTDC_LxCFBLNR_CFBLNBR_Pos, LTDC_LxCFBLNR_CFBLNBR_Msk);

  display_set_framebuffer(display, buffer);

  reg_set_bits(&display->layer->CR, LTDC_LxCR_LEN);

  display_reload(display);
}

void display_refresh(display_t *display) {
  reg_set_bits(&display->dsi->WCR, DSI_WCR_LTDCEN);
}

void display_reload(display_t *display) {
  reg_set_bits(&display->ltdc->SRCR, LTDC_SRCR_IMR);
}

void display_set_framebuffer(display_t *display, void* buffer) {
  reg_write_bits(&display->layer->CFBAR, ((uint32_t)buffer) << LTDC_LxCFBAR_CFBADD_Pos, LTDC_LxCFBAR_CFBADD_Msk);
}

void display_set_command_mode_transmission_kind(display_t *display,
                                                bool low_power) {
  reg_write_bits(&display->dsi->CMCR, low_power ? 0xFFFFFFFF : 0x00000000,
                 DSI_CMCR_GSW0TX | DSI_CMCR_GSW1TX | DSI_CMCR_GSW2TX |
                 DSI_CMCR_GSR0TX | DSI_CMCR_GSR1TX | DSI_CMCR_GSR2TX |
                 DSI_CMCR_GLWTX | DSI_CMCR_DSW0TX | DSI_CMCR_DSW1TX |
                 DSI_CMCR_DSR0TX | DSI_CMCR_DLWTX | DSI_CMCR_MRDPS);
}

void display_dsi_short_write(display_t *display, uint8_t lsb, uint8_t msb,
                             uint8_t discriminant) {
  while (!(display->dsi->GPSR & DSI_GPSR_CMDFE));

  display->dsi->GHCR = (msb << DSI_GHCR_WCMSB_Pos) | (lsb << DSI_GHCR_WCLSB_Pos) |
    (display->config.channel << DSI_GHCR_VCID_Pos) | (discriminant << DSI_GHCR_DT_Pos);
}

typedef union {
  uint32_t word;
  uint16_t shortword[2];
  uint8_t chars[4];
} dsi_long_data_t;

void display_dsi_long_write(display_t *display, uint8_t cmd, uint8_t *data,
                            uint16_t len, uint8_t discriminant) {
  while (!(display->dsi->GPSR & DSI_GPSR_CMDFE));

  dsi_long_data_t gpdr;
  gpdr.word = 0;
  gpdr.chars[0] = cmd;

  for (uint8_t i = 0; i < 3 && i < len; i++) {
    gpdr.chars[i + 1] = data[i];
  }

  display->dsi->GPDR = gpdr.word;

  for (uint8_t i = 3; i < len; i += 4) {
    for (uint8_t j = 0; j < 3 && i + j < len; j++) {
      gpdr.chars[j] = data[i + j];
    }
    display->dsi->GPDR = gpdr.word;
  }

  len++;
  display_dsi_short_write(display, (len & 0xFF), (len >> 8) & 0xFF, discriminant);
}

/* void display_dsi_read(display_t* display, uint8_t* buffer); */