/*********************************************************************************************************************
 * CH32V307VCT6 Opensourec Library ????CH32V307VCT6 ???????????????? SDK ??????????????
 * Copyright (c) 2022 SEEKFREE ?????
 *
 * ??????? CH32V307VCT6 ???????????
 *
 * CH32V307VCT6 ????? ?????????
 * ?????????????????????????? GPL??GNU General Public License???? GNU ??锟斤拷？???????????????
 * ?? GPL ??? 3 ?锟斤拷？? GPL3.0????????????锟绞︼拷？????锟斤拷？????锟斤拷？????/???????
 *
 * ????????????????????????????????锟斤拷？??????锟绞︼拷？??
 * ?????????????????????????????????
 * ?????????锟斤拷？GPL
 *
 * ?????????????????????????? GPL ?????
 * ?????锟斤拷？?????<https://www.gnu.org/licenses/>
 *
 * ?????????
 * ?????????? GPL3.0 ????????锟斤拷？? ?????????????????锟斤拷
 * ?????????????? libraries/doc ???????? GPL3_permission_statement.txt ?????
 * ??????????? libraries ??????? ???????????? LICENSE ???
 * ?????锟斤拷？?锟斤拷？?????????? ?????????????????????????????????????????
 *
 * ???????          main
 * ???????          ??????????????
 * ?锟斤拷？??          ?? libraries/doc ??????? version ??? ?锟斤拷？??
 * ????????          MounRiver Studio V1.8.1
 * ??????          CH32V307VCT6
 * ????????          https://seekfree.taobao.com/
 *
 * ?????
 * ????                                      ????                             ???
 * 2022-09-15        ?? W            first version
 ********************************************************************************************************************/
#include "zf_common_headfile.h"
#include "gl_headfile.h"
#include "by_pt_button.h"
#include "by_fan_control.h"
#include "cw_servo.h"
#include "./page/cw_page.h"

uint8_t (*Img_Gray)[MT9V03X_W];
int32_t pts_left[PT_MAXLEN][2];
int32_t pts_right[PT_MAXLEN][2];
int32_t pts_left_count, pts_right_count;
float pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2];
int32_t pts_inv_l_count, pts_inv_r_count;
float pts_filter_l[PT_MAXLEN][2], pts_filter_r[PT_MAXLEN][2];
int32_t pts_filter_l_count, pts_filter_r_count;
float pts_resample_left[PT_MAXLEN][2], pts_resample_right[PT_MAXLEN][2];
int32_t pts_resample_left_count, pts_resample_right_count;
float mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2];
int32_t mid_left_count, mid_right_count;

float angle_new_left[PT_MAXLEN];
float angle_new_right[PT_MAXLEN];
int angle_new_left_num, angle_new_right_num;
uint8_t mt9v03x_image_copy[MT9V03X_H][MT9V03X_W];

float angle_left[PT_MAXLEN];
float angle_right[PT_MAXLEN];
int angle_left_num, angle_right_num;

int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
bool Lpt0_found, Lpt1_found;
int Lpt1[2], Lpt0[2];

int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id;
bool Lpt_in0_found, Lpt_in1_found;
int Lpt_in1[2], Lpt_in0[2];

bool is_straight0, is_straight1;

bool is_turn0, is_turn1;

float rptsn[PT_MAXLEN][2];
int rptsn_num;
float aim_distance;

enum track_type_e track_type = TRACK_RIGHT;

int frame_count = 0;

void img_processing();
void get_corners();
void adaptiveThreshold(uint8_t* img_data, uint8_t* output_data, int width, int height, int block, uint8_t clip_value);



int main(void)
{
    clock_init(SYSTEM_CLOCK_120M); // 初始化芯片时钟 工作频率为 120MHz
    debug_init();                  // 务必保留，本函数用于初始化 MPU 时钟 调试串口
    //mt9v03x_init();
    // system_delay_ms(2000);
    ips200_init(IPS200_TYPE_SPI);
    mt9v03x_init();
    // by_gpio_init();
    // by_exit_init();
    // by_pwm_init();
    // cw_servo_init();
    //printf("hello world\n");
    // while (imu660ra_init())
    //     ;

    // system_delay_ms(2000);
    // gyroOffset_init();
    // pit_ms_init(TIM6_PIT, 2);

    while (1) {
        // while (frame_count < 20) {
        //  if (mt9v03x_finish_flag) {
        //      memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
        //      adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 8);
        //      //threshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, MT9V03X_W, MT9V03X_H, 110);
        //      ips200_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0);
        //      mt9v03x_finish_flag = 0;
        //      frame_count++;
        //  }

        //}
        if (mt9v03x_finish_flag) {
            memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
            adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 17);
            ips200_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0);
            mt9v03x_finish_flag = 0;

            state_type = COMMON_STATE;
            img_processing();

            //get_corners();

            //aim_distance = COMMON_AIM;

            //tracking();
            //printf("1\r\n");
            //system_delay_ms(100);

            //ElementJudge();

            //ElementRun();

            //MidLineTrack();
        }
    }
}



void adaptiveThreshold(uint8_t* img_data, uint8_t* output_data, int width, int height, int block, uint8_t clip_value){
  int half_block = block / 2;
  for(int y=half_block; y<height-half_block; y++){
    for(int x=half_block; x<width-half_block; x++){
      // 计算局部阈值
      int thres = 0;
      for(int dy=-half_block; dy<=half_block; dy++){
        for(int dx=-half_block; dx<=half_block; dx++){
          thres += img_data[(x+dx)+(y+dy)*width];
        }
      }
      thres = thres / (block * block) - clip_value;
      // 进行二值化
      output_data[x+y*width] = img_data[x+y*width]>thres ? 255 : 0;
    }
  }
}