/*********************************************************************************************************************
 * 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 "cw_servo.h"
#include "by_pt_button.h"
#include "by_fan_control.h"
#include "by_imu.h"

uint8 (*Img_Gray)[MT9V03X_W]; // 定义指向包含 MT9V03X_W 列的 uint8 类型的二维数组的指针
// uint8 *mt9v03x_image_copy[0];               // 定义指向 uint8 类型的一维数组的指针
sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2];
sint32 pts_left_count, pts_right_count;
float32 pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2];
sint32 pts_inv_l_count, pts_inv_r_count;
float32 pts_filter_l[PT_MAXLEN][2], pts_filter_r[PT_MAXLEN][2];
sint32 pts_filter_l_count, pts_filter_r_count;
float32 pts_resample_left[PT_MAXLEN][2], pts_resample_right[PT_MAXLEN][2];
sint32 pts_resample_left_count, pts_resample_right_count;
float32 mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2];
sint32 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;

// L角点
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();

int main(void)
{
    clock_init(SYSTEM_CLOCK_120M); // 初始化芯片时钟 工作频率为 120MHz
    debug_init();                  // 务必保留，本函数用于初始化MPU 时钟 调试串口
    // mt9v03x_init();
    ips200_init(IPS200_TYPE_SPI);
    by_gpio_init();
    by_exit_init();
    by_pwm_init();
    cw_servo_init();
    while (imu660ra_init())
        ;
    gyroOffset_init();
    pit_ms_init(TIM6_PIT, 1);

    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);
        //      ips114_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) {
            // ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0);
            memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
            // Img_Gray = mt9v03x_image;
            // mt9v03x_image_copy[0] = Img_Gray[0];
            mt9v03x_finish_flag = 0;

            state_type = COMMON_STATE;
            img_processing();

            get_corners();

            aim_distance = COMMON_AIM;

            tracking();

            ElementJudge();

            ElementRun();

            MidLineTrack();
        }
        // by_imu_data_get();
        by_ips_show();
        system_delay_ms(200);
    }
}