/*********************************************************************************************************************
 * 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];
// uint8_t *mt9v03x_image_copy[0];
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();

int main(void)
{
    clock_init(SYSTEM_CLOCK_120M);
    debug_init();
    mt9v03x_init();
    ips200_init(IPS200_TYPE_SPI);
    by_gpio_init();
    by_exit_init();
    by_pwm_init();
    cw_servo_init();
    // while (imu660ra_init())
    //     ;

    Page_Init();

    while (1) {
        Page_Run();
        
        if (mt9v03x_finish_flag) {
            memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
            mt9v03x_finish_flag = 0;

            state_type = COMMON_STATE;
            img_processing();
            get_corners();
            aim_distance = COMMON_AIM;
            tracking();
            ElementJudge();
            ElementRun();
            MidLineTrack();
        }
    }
}