/*=============================================================================
    文件名：VL53L0X_LCD_ST7735_Fixed.c
    描  述：VL53L0X 测距 + ST7735 显示，软件 I2C + 软件 SPI，
    硬件连接：
        VL53L0X: SDA - P1.4, SCL - P1.5, VCC - 3.3V, GND - GND
        ST7735:  SCL - P2.5, SDA - P2.3, RES - P2.0, DC - P2.1, CS - P2.2, BL - P2.7
    主时钟：22.1184MHz
=============================================================================*/

#include "STC32G.h"

typedef unsigned char   u8;
typedef unsigned int    u16;
typedef unsigned long   u32;

#define MAIN_Fosc        22118400L

//==========================================================================
// 软件 I2C 引脚定义（VL53L0X）
sbit SDA = P1^4;
sbit SCL = P1^5;

// 软件 SPI 引脚定义（ST7735）
sbit LCD_SCL = P2^5;
sbit LCD_SDA = P2^3;
sbit LCD_RES = P2^0;
sbit LCD_DC  = P2^1;
sbit LCD_CS  = P2^2;
sbit LCD_BL  = P2^7;

//==========================================================================
// 8x16 ASCII 字库（ASC16）
const unsigned char ASC16[][16] = {
    /* 32    */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 33  ! */{ 0x00,0x00,0x00,0x18,0x3C,0x3C,0x3C,0x18,0x18,0x18,0x00,0x18,0x18,0x00,0x00,0x00, },
    /* 34  " */{ 0x00,0x00,0x66,0x66,0x66,0x24,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 35  # */{ 0x00,0x00,0x00,0x00,0x6C,0x6C,0xFE,0x6C,0x6C,0x6C,0xFE,0x6C,0x6C,0x00,0x00,0x00, },
    /* 36  $ */{ 0x00,0x18,0x18,0x7C,0xC6,0xC2,0xC0,0x7C,0x06,0x06,0x86,0xC6,0x7C,0x18,0x18,0x00, },
    /* 37  % */{ 0x00,0x00,0x00,0x00,0x00,0xC2,0xC6,0x0C,0x18,0x30,0x60,0xC6,0x86,0x00,0x00,0x00, },
    /* 38  & */{ 0x00,0x00,0x00,0x38,0x6C,0x6C,0x38,0x76,0xDC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00, },
    /* 39  ' */{ 0x00,0x00,0x30,0x30,0x30,0x60,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 40  ( */{ 0x00,0x00,0x00,0x0C,0x18,0x30,0x30,0x30,0x30,0x30,0x30,0x18,0x0C,0x00,0x00,0x00, },
    /* 41  ) */{ 0x00,0x00,0x00,0x30,0x18,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x18,0x30,0x00,0x00,0x00, },
    /* 42  * */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x66,0x3C,0xFF,0x3C,0x66,0x00,0x00,0x00,0x00,0x00, },
    /* 43  + */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x7E,0x18,0x18,0x00,0x00,0x00,0x00,0x00, },
    /* 44  , */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x18,0x30,0x00,0x00, },
    /* 45  - */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 46  . */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00, },
    /* 47  / */{ 0x00,0x00,0x00,0x00,0x00,0x02,0x06,0x0C,0x18,0x30,0x60,0xC0,0x80,0x00,0x00,0x00, },
    /* 48  0 */{ 0x00,0x00,0x00,0x38,0x6C,0xC6,0xC6,0xD6,0xD6,0xC6,0xC6,0x6C,0x38,0x00,0x00,0x00, },
    /* 49  1 */{ 0x00,0x00,0x00,0x18,0x38,0x78,0x18,0x18,0x18,0x18,0x18,0x18,0x7E,0x00,0x00,0x00, },
    /* 50  2 */{ 0x00,0x00,0x00,0x7C,0xC6,0x06,0x0C,0x18,0x30,0x60,0xC0,0xC6,0xFE,0x00,0x00,0x00, },
    /* 51  3 */{ 0x00,0x00,0x00,0x7C,0xC6,0x06,0x06,0x3C,0x06,0x06,0x06,0xC6,0x7C,0x00,0x00,0x00, },
    /* 52  4 */{ 0x00,0x00,0x00,0x0C,0x1C,0x3C,0x6C,0xCC,0xFE,0x0C,0x0C,0x0C,0x1E,0x00,0x00,0x00, },
    /* 53  5 */{ 0x00,0x00,0x00,0xFE,0xC0,0xC0,0xC0,0xFC,0x06,0x06,0x06,0xC6,0x7C,0x00,0x00,0x00, },
    /* 54  6 */{ 0x00,0x00,0x00,0x38,0x60,0xC0,0xC0,0xFC,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00, },
    /* 55  7 */{ 0x00,0x00,0x00,0xFE,0xC6,0x06,0x06,0x0C,0x18,0x30,0x30,0x30,0x30,0x00,0x00,0x00, },
    /* 56  8 */{ 0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0x7C,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00, },
    /* 57  9 */{ 0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0x7E,0x06,0x06,0x06,0x0C,0x78,0x00,0x00,0x00, },
    /* 58  : */{ 0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x00, },
    /* 59  ; */{ 0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x18,0x18,0x30,0x00,0x00,0x00, },
    /* 60  < */{ 0x00,0x00,0x00,0x00,0x06,0x0C,0x18,0x30,0x60,0x30,0x18,0x0C,0x06,0x00,0x00,0x00, },
    /* 61  = */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x7E,0x00,0x00,0x7E,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 62  > */{ 0x00,0x00,0x00,0x00,0x60,0x30,0x18,0x0C,0x06,0x0C,0x18,0x30,0x60,0x00,0x00,0x00, },
    /* 63  ? */{ 0x00,0x00,0x00,0x7C,0xC6,0xC6,0x0C,0x18,0x18,0x18,0x00,0x18,0x18,0x00,0x00,0x00, },
    /* 64  @ */{ 0x00,0x00,0x00,0x00,0x7C,0xC6,0xC6,0xDE,0xDE,0xDE,0xDC,0xC0,0x7C,0x00,0x00,0x00, },
    /* 65  A */{ 0x00,0x00,0x00,0x10,0x38,0x6C,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00, },
    /* 66  B */{ 0x00,0x00,0x00,0xFC,0x66,0x66,0x66,0x7C,0x66,0x66,0x66,0x66,0xFC,0x00,0x00,0x00, },
    /* 67  C */{ 0x00,0x00,0x00,0x3C,0x66,0xC2,0xC0,0xC0,0xC0,0xC0,0xC2,0x66,0x3C,0x00,0x00,0x00, },
    /* 68  D */{ 0x00,0x00,0x00,0xF8,0x6C,0x66,0x66,0x66,0x66,0x66,0x66,0x6C,0xF8,0x00,0x00,0x00, },
    /* 69  E */{ 0x00,0x00,0x00,0xFE,0x66,0x62,0x68,0x78,0x68,0x60,0x62,0x66,0xFE,0x00,0x00,0x00, },
    /* 70  F */{ 0x00,0x00,0x00,0xFE,0x66,0x62,0x68,0x78,0x68,0x60,0x60,0x60,0xF0,0x00,0x00,0x00, },
    /* 71  G */{ 0x00,0x00,0x00,0x3C,0x66,0xC2,0xC0,0xC0,0xDE,0xC6,0xC6,0x66,0x3A,0x00,0x00,0x00, },
    /* 72  H */{ 0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00, },
    /* 73  I */{ 0x00,0x00,0x00,0x3C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00, },
    /* 74  J */{ 0x00,0x00,0x00,0x1E,0x0C,0x0C,0x0C,0x0C,0x0C,0xCC,0xCC,0xCC,0x78,0x00,0x00,0x00, },
    /* 75  K */{ 0x00,0x00,0x00,0xE6,0x66,0x66,0x6C,0x78,0x78,0x6C,0x66,0x66,0xE6,0x00,0x00,0x00, },
    /* 76  L */{ 0x00,0x00,0x00,0xF0,0x60,0x60,0x60,0x60,0x60,0x60,0x62,0x66,0xFE,0x00,0x00,0x00, },
    /* 77  M */{ 0x00,0x00,0x00,0xC6,0xEE,0xFE,0xFE,0xD6,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00, },
    /* 78  N */{ 0x00,0x00,0x00,0xC6,0xE6,0xF6,0xFE,0xDE,0xCE,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00, },
    /* 79  O */{ 0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00, },
    /* 80  P */{ 0x00,0x00,0x00,0xFC,0x66,0x66,0x66,0x7C,0x60,0x60,0x60,0x60,0xF0,0x00,0x00,0x00, },
    /* 81  Q */{ 0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xD6,0xDE,0x7C,0x0C,0x0E,0x00,0x00, },
    /* 82  R */{ 0x00,0x00,0x00,0xFC,0x66,0x66,0x66,0x7C,0x6C,0x66,0x66,0x66,0xE6,0x00,0x00,0x00, },
    /* 83  S */{ 0x00,0x00,0x00,0x7C,0xC6,0xC6,0x60,0x38,0x0C,0x06,0xC6,0xC6,0x7C,0x00,0x00,0x00, },
    /* 84  T */{ 0x00,0x00,0x00,0x7E,0x7E,0x5A,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00, },
    /* 85  U */{ 0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00, },
    /* 86  V */{ 0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x6C,0x38,0x10,0x00,0x00,0x00, },
    /* 87  W */{ 0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xD6,0xD6,0xD6,0xFE,0xEE,0x6C,0x00,0x00,0x00, },
    /* 88  X */{ 0x00,0x00,0x00,0xC6,0xC6,0x6C,0x7C,0x38,0x38,0x7C,0x6C,0xC6,0xC6,0x00,0x00,0x00, },
    /* 89  Y */{ 0x00,0x00,0x00,0x66,0x66,0x66,0x66,0x3C,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00, },
    /* 90  Z */{ 0x00,0x00,0x00,0xFE,0xC6,0x86,0x0C,0x18,0x30,0x60,0xC2,0xC6,0xFE,0x00,0x00,0x00, },
    /* 91  [ */{ 0x00,0x00,0x00,0x3C,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x3C,0x00,0x00,0x00, },
    /* 92  \ */{ 0x00,0x00,0x00,0x00,0x80,0xC0,0xE0,0x70,0x38,0x1C,0x0E,0x06,0x02,0x00,0x00,0x00, },
    /* 93  ] */{ 0x00,0x00,0x00,0x3C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x3C,0x00,0x00,0x00, },
    /* 94  ^ */{ 0x00,0x10,0x38,0x6C,0xC6,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 95  _ */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00, },
    /* 96  ` */{ 0x00,0x00,0x18,0x3C,0x66,0x66,0x3C,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 97  a */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x78,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00, },
    /* 98  b */{ 0x00,0x00,0x00,0xE0,0x60,0x60,0x78,0x6C,0x66,0x66,0x66,0x66,0x7C,0x00,0x00,0x00, },
    /* 99  c */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xC0,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00, },
    /* 100 d */{ 0x00,0x00,0x00,0x1C,0x0C,0x0C,0x3C,0x6C,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00, },
    /* 101 e */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xFE,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00, },
    /* 102 f */{ 0x00,0x00,0x00,0x38,0x6C,0x64,0x60,0xF0,0x60,0x60,0x60,0x60,0xF0,0x00,0x00,0x00, },
    /* 103 g */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x76,0xCC,0xCC,0xCC,0xCC,0xCC,0x7C,0x0C,0xCC,0x78, },
    /* 104 h */{ 0x00,0x00,0x00,0xE0,0x60,0x60,0x6C,0x76,0x66,0x66,0x66,0x66,0xE6,0x00,0x00,0x00, },
    /* 105 i */{ 0x00,0x00,0x00,0x18,0x18,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00, },
    /* 106 j */{ 0x00,0x00,0x00,0x06,0x06,0x00,0x0E,0x06,0x06,0x06,0x06,0x06,0x06,0x66,0x66,0x3C, },
    /* 107 k */{ 0x00,0x00,0x00,0xE0,0x60,0x60,0x66,0x6C,0x78,0x78,0x6C,0x66,0xE6,0x00,0x00,0x00, },
    /* 108 l */{ 0x00,0x00,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00, },
    /* 109 m */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xEC,0xFE,0xD6,0xD6,0xD6,0xD6,0xC6,0x00,0x00,0x00, },
    /* 110 n */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xDC,0x66,0x66,0x66,0x66,0x66,0x66,0x00,0x00,0x00, },
    /* 111 o */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00, },
    /* 112 p */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xDC,0x66,0x66,0x66,0x66,0x66,0x7C,0x60,0x60,0xF0, },
    /* 113 q */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x76,0xCC,0xCC,0xCC,0xCC,0xCC,0x7C,0x0C,0x0C,0x1E, },
    /* 114 r */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xDC,0x76,0x66,0x60,0x60,0x60,0xF0,0x00,0x00,0x00, },
    /* 115 s */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0x60,0x38,0x0C,0xC6,0x7C,0x00,0x00,0x00, },
    /* 116 t */{ 0x00,0x00,0x00,0x10,0x30,0x30,0xFC,0x30,0x30,0x30,0x30,0x36,0x1C,0x00,0x00,0x00, },
    /* 117 u */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00, },
    /* 118 v */{ 0x00,0x00,0x00,0x00,0x00,0x00,0x66,0x66,0x66,0x66,0x66,0x3C,0x18,0x00,0x00,0x00, },
    /* 119 w */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xC6,0xC6,0xD6,0xD6,0xD6,0xFE,0x6C,0x00,0x00,0x00, },
    /* 120 x */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xC6,0x6C,0x38,0x38,0x38,0x6C,0xC6,0x00,0x00,0x00, },
    /* 121 y */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7E,0x06,0x0C,0xF8, },
    /* 122 z */{ 0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0xCC,0x18,0x30,0x60,0xC6,0xFE,0x00,0x00,0x00, },
    /* 123 { */{ 0x00,0x00,0x00,0x0E,0x18,0x18,0x18,0x70,0x18,0x18,0x18,0x18,0x0E,0x00,0x00,0x00, },
    /* 124 | */{ 0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x00,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00, },
    /* 125 } */{ 0x00,0x00,0x00,0x70,0x18,0x18,0x18,0x0E,0x18,0x18,0x18,0x18,0x70,0x00,0x00,0x00, },
    /* 126 ~ */{ 0x00,0x00,0x00,0x76,0xDC,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, },
    /* 127  */{ 0x00,0x00,0x00,0x00,0x00,0x10,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0x00,0x00,0x00,0x00, },
};

// 颜色表（16位RGB565）
const u16 COLOR[] = {
    0x0000, 0x1048, 0x1442, 0x1452, 0x9042, 0x9052, 0xA484, 0x8410,
    0xC618, 0x001F, 0x07E0, 0x07FF, 0xF800, 0xF81F, 0xFFE0, 0xFFFF,
    0x2104, 0x0006, 0x79EF, 0x8408, 0x0180, 0x041F, 0x0210, 0x801F,
    0x8200, 0xF81F, 0xFFF0, 0x0700, 0xC7FF, 0x841F, 0xF810, 0xFC08,
};

//==========================================================================
// 延时函数（微秒和毫秒）
void delay_us(u16 us)
{
    u16 i;
    while (us--) {
        for (i = 0; i < MAIN_Fosc / 1000000 / 3; i++);
    }
}

void delay_ms(u16 ms)
{
    while (ms--) delay_us(1000);
}

//==========================================================================
// 软件 I2C 函数（VL53L0X）
void I2C_Start(void)
{
    SDA = 1; SCL = 1; delay_us(5);
    SDA = 0; delay_us(5);
    SCL = 0; delay_us(5);
}

void I2C_Stop(void)
{
    SDA = 0; SCL = 1; delay_us(5);
    SDA = 1; delay_us(5);
}

void I2C_SendByte(u8 dat)
{
    u8 i;
    for (i = 0; i < 8; i++) {
        if (dat & 0x80) SDA = 1;
        else            SDA = 0;
        dat <<= 1;
        delay_us(2);
        SCL = 1; delay_us(2);
        SCL = 0; delay_us(2);
    }
    SDA = 1; delay_us(2);
    SCL = 1; delay_us(2);
}

u8 I2C_CheckAck(void)
{
    u8 ack = 0;
    if (SDA == 0) ack = 1;
    SCL = 0; delay_us(2);
    return ack;
}

void I2C_SendAck(void)
{
    SDA = 0; delay_us(2);
    SCL = 1; delay_us(2);
    SCL = 0; delay_us(2);
    SDA = 1;
}

void I2C_SendNak(void)
{
    SDA = 1; delay_us(2);
    SCL = 1; delay_us(2);
    SCL = 0; delay_us(2);
}

u8 I2C_ReadByte(void)
{
    u8 i, dat = 0;
    SDA = 1;
    for (i = 0; i < 8; i++) {
        dat <<= 1;
        SCL = 1; delay_us(2);
        if (SDA) dat |= 1;
        SCL = 0; delay_us(2);
    }
    return dat;
}

// VL53L0X 操作
#define VL53L0X_ADDR     0x29
#define REG_IDENTIFICATION_MODEL_ID         0xC0
#define REG_IDENTIFICATION_REVISION_ID      0xC2
#define REG_PRE_RANGE_CONFIG_VCSEL_PERIOD   0x50
#define REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define REG_SYSRANGE_START                  0x00
#define REG_RESULT_RANGE_STATUS              0x14

u8 gbuf[16];

void write_byte_data_at(u8 reg, u8 dat)
{
    I2C_Start();
    I2C_SendByte((VL53L0X_ADDR << 1) | 0);
    I2C_CheckAck();
    I2C_SendByte(reg);
    I2C_CheckAck();
    I2C_SendByte(dat);
    I2C_CheckAck();
    I2C_Stop();
}

u8 read_byte_data_at(u8 reg)
{
    u8 res;
    I2C_Start();
    I2C_SendByte((VL53L0X_ADDR << 1) | 0);
    I2C_CheckAck();
    I2C_SendByte(reg);
    I2C_CheckAck();
    I2C_Stop();

    I2C_Start();
    I2C_SendByte((VL53L0X_ADDR << 1) | 1);
    I2C_CheckAck();
    res = I2C_ReadByte();
    I2C_SendNak();
    I2C_Stop();
    return res;
}

void read_block_data_at(u8 reg, u8 len, u8 *buf)
{
    u8 i;
    I2C_Start();
    I2C_SendByte((VL53L0X_ADDR << 1) | 0);
    I2C_CheckAck();
    I2C_SendByte(reg);
    I2C_CheckAck();
    I2C_Stop();

    I2C_Start();
    I2C_SendByte((VL53L0X_ADDR << 1) | 1);
    I2C_CheckAck();
    for (i = 0; i < len; i++) {
        buf[i] = I2C_ReadByte();
        if (i == len - 1) I2C_SendNak();
        else I2C_SendAck();
    }
    I2C_Stop();
}

u16 makeuint16(u8 lsb, u8 msb)
{
    return ((u16)msb << 8) | lsb;
}

u16 VL53L0X_decode_vcsel_period(u8 vcsel_period_reg)
{
    return (vcsel_period_reg + 1) << 1;
}

//==========================================================================
// 软件 SPI 函数（ST7735）
void LCD_SPI_WriteByte(u8 dat)
{
    u8 i;
    for (i = 0; i < 8; i++) {
        if (dat & 0x80) LCD_SDA = 1;
        else            LCD_SDA = 0;
        dat <<= 1;
        LCD_SCL = 0;
        delay_us(1);
        LCD_SCL = 1;
        delay_us(1);
    }
}

void LCD_WriteCmd(u8 cmd)
{
    LCD_CS = 0;
    LCD_DC = 0;
    LCD_SPI_WriteByte(cmd);
    LCD_CS = 1;
}

void LCD_WriteData(u8 dat)
{
    LCD_CS = 0;
    LCD_DC = 1;
    LCD_SPI_WriteByte(dat);
    LCD_CS = 1;
}

void LCD_WriteData16(u16 dat)
{
    LCD_CS = 0;
    LCD_DC = 1;
    LCD_SPI_WriteByte((u8)(dat >> 8));
    LCD_SPI_WriteByte((u8)dat);
    LCD_CS = 1;
}

void LCD_SetWindow(u8 x, u8 y, u8 w, u8 h)
{
    LCD_WriteCmd(0x2A);
    LCD_WriteData(0); LCD_WriteData(x);
    LCD_WriteData(0); LCD_WriteData(x + w - 1);
    LCD_WriteCmd(0x2B);
    LCD_WriteData(0); LCD_WriteData(y);
    LCD_WriteData(0); LCD_WriteData(y + h - 1);
    LCD_WriteCmd(0x2C);
}

void LCD_Init(void)
{
    // 复位
    LCD_RES = 0; delay_ms(120);
    LCD_RES = 1; delay_ms(100);

    LCD_WriteCmd(0x11); delay_ms(120);

    // 帧率
    LCD_WriteCmd(0xB1); LCD_WriteData(0x01); LCD_WriteData(0x2C); LCD_WriteData(0x2D);
    LCD_WriteCmd(0xB2); LCD_WriteData(0x01); LCD_WriteData(0x2C); LCD_WriteData(0x2D);
    LCD_WriteCmd(0xB3); LCD_WriteData(0x01); LCD_WriteData(0x2C); LCD_WriteData(0x2D);
    LCD_WriteData(0x01); LCD_WriteData(0x2C); LCD_WriteData(0x2D);
    LCD_WriteCmd(0xB4); LCD_WriteData(0x07);

    // 电源
    LCD_WriteCmd(0xC0); LCD_WriteData(0xA2); LCD_WriteData(0x02); LCD_WriteData(0x84);
    LCD_WriteCmd(0xC1); LCD_WriteData(0xC5);
    LCD_WriteCmd(0xC2); LCD_WriteData(0x0A); LCD_WriteData(0x00);
    LCD_WriteCmd(0xC3); LCD_WriteData(0x8A); LCD_WriteData(0x2A);
    LCD_WriteCmd(0xC4); LCD_WriteData(0x8A); LCD_WriteData(0xEE);
    LCD_WriteCmd(0xC5); LCD_WriteData(0x0E);

    // 方向（竖屏）
    LCD_WriteCmd(0x36); LCD_WriteData(0xC0);  // 如果颜色异常可改为 0xC8

    // Gamma
    LCD_WriteCmd(0xE0); LCD_WriteData(0x0F); LCD_WriteData(0x1A); LCD_WriteData(0x0F); LCD_WriteData(0x18);
    LCD_WriteData(0x2F); LCD_WriteData(0x28); LCD_WriteData(0x20); LCD_WriteData(0x22);
    LCD_WriteData(0x1F); LCD_WriteData(0x1B); LCD_WriteData(0x23); LCD_WriteData(0x37);
    LCD_WriteData(0x00); LCD_WriteData(0x07); LCD_WriteData(0x02); LCD_WriteData(0x10);
    LCD_WriteCmd(0xE1); LCD_WriteData(0x0F); LCD_WriteData(0x1B); LCD_WriteData(0x0F); LCD_WriteData(0x17);
    LCD_WriteData(0x33); LCD_WriteData(0x2C); LCD_WriteData(0x29); LCD_WriteData(0x2E);
    LCD_WriteData(0x30); LCD_WriteData(0x30); LCD_WriteData(0x39); LCD_WriteData(0x3F);
    LCD_WriteData(0x00); LCD_WriteData(0x07); LCD_WriteData(0x03); LCD_WriteData(0x10);

    // 设置显示窗口
    LCD_WriteCmd(0x2A); LCD_WriteData(0x00); LCD_WriteData(0x00); LCD_WriteData(0x00); LCD_WriteData(0x7F);
    LCD_WriteCmd(0x2B); LCD_WriteData(0x00); LCD_WriteData(0x00); LCD_WriteData(0x00); LCD_WriteData(0x9F);

    LCD_WriteCmd(0xF0); LCD_WriteData(0x01);
    LCD_WriteCmd(0xF6); LCD_WriteData(0x00);
    LCD_WriteCmd(0x3A); LCD_WriteData(0x05);
    LCD_WriteCmd(0x29);

    LCD_BL = 1;  // 背光
}

void LCD_Clear(u8 color_index)
{
    u16 i;
    LCD_SetWindow(0, 0, 128, 160);
    for (i = 0; i < 128 * 160; i++) {
        LCD_WriteData16(COLOR[color_index]);
    }
}

void LCD_SetCursor(u16 x, u16 y)
{
    LCD_WriteCmd(0x2A);
    LCD_WriteData((u8)(x >> 8)); LCD_WriteData((u8)x);
    LCD_WriteData(0); LCD_WriteData(127);
    LCD_WriteCmd(0x2B);
    LCD_WriteData((u8)(y >> 8)); LCD_WriteData((u8)y);
    LCD_WriteData(0); LCD_WriteData(159);
    LCD_WriteCmd(0x2C);
}

void LCD_ShowString(u16 x, u16 y, char *str, u8 fg_color, u8 bg_color, u8 len)
{
    u8 i, j, k;
    for (k = 0; k < len; k++) {
        u8 ch = str[k] - 32;
        for (i = 0; i < 16; i++) {
            u8 line = ASC16[ch][i];
            LCD_SetCursor(x + k * 8, y + i);
            for (j = 0; j < 8; j++) {
                if (line & 0x80)
                    LCD_WriteData16(COLOR[fg_color]);
                else
                    LCD_WriteData16(COLOR[bg_color]);
                line <<= 1;
            }
        }
    }
}

//==========================================================================
// 整数转字符串
void int_to_str(u16 num, char *str)
{
    char buf[6];
    int i = 0, j;
    if (num == 0) {
        str[0] = '0';
        str[1] = '\0';
        return;
    }
    while (num > 0) {
        buf[i++] = '0' + (num % 10);
        num /= 10;
    }
    for (j = 0; j < i; j++) {
        str[j] = buf[i - 1 - j];
    }
    str[i] = '\0';
}

//==========================================================================
// 主函数
void main(void)
{
    u16 dist;
    char disp_str[20];
    u8 val;
    u16 timeout;
    u8 len;   // 声明移到开头

    WTST = 0; EAXFR = 1; CKCON = 0;

    // 设置 I2C 引脚（P1.4,P1.5）为准双向并启用上拉
    P1M1 &= ~0x30; P1M0 &= ~0x30;
    P1PU |= 0x30;

    // 设置 LCD 引脚为准双向（默认即可）
    P2M1 &= ~0xEF;  // 清空相关位，保持默认
    P2M0 &= ~0xEF;

    // 初始化外设
    LCD_Init();
    LCD_Clear(0);   // 黑色清屏

    // 验证传感器 ID（可选）
    val = read_byte_data_at(REG_IDENTIFICATION_MODEL_ID);
    if (val != 0xEE) {
        // 若 ID 不对，在 LCD 上显示错误
        LCD_ShowString(10, 10, "Sensor Error", 15, 0, 12);
        while (1);
    }

    while (1)
    {
        // 启动单次测量
        write_byte_data_at(REG_SYSRANGE_START, 0x01);

        // 等待测量完成
        timeout = 0;
        do {
            delay_ms(10);
            val = read_byte_data_at(REG_RESULT_RANGE_STATUS);
            if (val & 0x01) break;
            timeout++;
        } while (timeout < 20);

        if (val & 0x01) {
            // 读取距离
            read_block_data_at(REG_RESULT_RANGE_STATUS, 12, gbuf);
            dist = makeuint16(gbuf[11], gbuf[10]);

            // 转换为字符串
            int_to_str(dist, disp_str);

            // 添加 " mm"
            len = 0;
            while (disp_str[len]) len++;
            disp_str[len] = 'm';
            disp_str[len+1] = 'm';
            disp_str[len+2] = '\0';

            // 显示在 LCD 上
            LCD_ShowString(10, 10, disp_str, 15, 0, len+2);
        } else {
            LCD_ShowString(10, 10, "Timeout", 15, 0, 7);
        }

        delay_ms(500); // 稍作延时
    }
}