/*
 * Copyright (C) 2023 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */

#include <stdio.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"

#include "hi_io.h"
#include "iot_gpio.h"

#include "pzcar_i2c_ctrl.h"
#include "pzcar_led_ctrl.h"
#include "pzcar_wheel_ctrl.h"

extern PzCarInfo g_carInfo;

#define SEQ_LEN  (36)
#define ITEM_LEN (4)

void LedCheck(void)
{
    uint8_t ledCheckSeq[SEQ_LEN][ITEM_LEN] =
    {
        {0x1E, 0x00, 0x00, 0x00},  //LED1 Off
        {0x1E, 0xFF, 0x00, 0x00},  //LED1 On-Red
        {0x1E, 0x00, 0xFF, 0x00},  //LED1 On-Green
        {0x1E, 0x00, 0x00, 0xFF},  //LED1 On-Blue
        {0x1E, 0xFF, 0xFF, 0xFF},  //LED1 On-White
        {0x1E, 0x00, 0x00, 0x00},  //LED1 Off

        {0x21, 0x00, 0x00, 0x00},  //LED2 Off
        {0x21, 0xFF, 0x00, 0x00},  //LED2 On-Red
        {0x21, 0x00, 0xFF, 0x00},  //LED2 On-Green
        {0x21, 0x00, 0x00, 0xFF},  //LED2 On-Blue
        {0x21, 0xFF, 0xFF, 0xFF},  //LED2 On-White
        {0x21, 0x00, 0x00, 0x00},  //LED2 Off

        {0x24, 0x00, 0x00, 0x00},  //RGB0 Off
        {0x24, 0xFF, 0x00, 0x00},  //RGB0 On-Red
        {0x24, 0x00, 0xFF, 0x00},  //RGB0 On-Green
        {0x24, 0x00, 0x00, 0xFF},  //RGB0 On-Blue
        {0x24, 0xFF, 0xFF, 0xFF},  //RGB0 On-White
        {0x24, 0x00, 0x00, 0x00},  //RGB0 Off

        {0x27, 0x00, 0x00, 0x00},  //RGB1 Off
        {0x27, 0xFF, 0x00, 0x00},  //RGB1 On-Red
        {0x27, 0x00, 0xFF, 0x00},  //RGB1 On-Green
        {0x27, 0x00, 0x00, 0xFF},  //RGB1 On-Blue
        {0x27, 0xFF, 0xFF, 0xFF},  //RGB1 On-White
        {0x27, 0x00, 0x00, 0x00},  //RGB1 Off

        {0x2A, 0x00, 0x00, 0x00},  //RGB2 Off
        {0x2A, 0xFF, 0x00, 0x00},  //RGB2 On-Red
        {0x2A, 0x00, 0xFF, 0x00},  //RGB2 On-Green
        {0x2A, 0x00, 0x00, 0xFF},  //RGB2 On-Blue
        {0x2A, 0xFF, 0xFF, 0xFF},  //RGB2 On-White
        {0x2A, 0x00, 0x00, 0x00},  //RGB2 Off

        {0x2D, 0x00, 0x00, 0x00},  //RGB3 Off
        {0x2D, 0xFF, 0x00, 0x00},  //RGB3 On-Red
        {0x2D, 0x00, 0xFF, 0x00},  //RGB3 On-Green
        {0x2D, 0x00, 0x00, 0xFF},  //RGB3 On-Blue
        {0x2D, 0xFF, 0xFF, 0xFF},  //RGB3 On-White
        {0x2D, 0x00, 0x00, 0x00},  //RGB3 Off
    };

    for (unsigned char i = 0; i < SEQ_LEN; i++) {
        I2C0_Write(ledCheckSeq[i], ITEM_LEN);
        usleep(100000);
    }
}

void LedCtrl(void)
{
    while (g_carInfo.e_ST == eSt_TurnLeft) {
        uint8_t ledOn[4]  = {0x1E, 0xFF, 0xFF, 0x00};
        I2C0_Write(ledOn, 4);
        usleep(200*1000);
        uint8_t ledOff[4] = {0x1E, 0x00, 0x00, 0x00};
        I2C0_Write(ledOff, 4);
        usleep(200*1000);
    }

    while (g_carInfo.e_ST == eSt_TurnRight) {
        uint8_t ledOn[4]  = {0x21, 0xFF, 0xFF, 0x00};
        I2C0_Write(ledOn, 4);
        usleep(200*1000);
        uint8_t ledOff[4] = {0x21, 0x00, 0x00, 0x00};
        I2C0_Write(ledOff, 4);
        usleep(200*1000);
    }
    
    while (g_carInfo.e_ST == eSt_Backward) {
        uint8_t ledOn[7]  = {0x24, 0xFF, 0x00, 0x00, 0xFF, 0x00, 0x00};
        I2C0_Write(ledOn, 7);   // RGB0+RGB1: Red
        usleep(200*1000);
        uint8_t ledOff[7]  = {0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
        I2C0_Write(ledOff, 7);
        usleep(200*1000);
    }
    
    while (g_carInfo.e_ST == eSt_BackLeft) {
        uint8_t ledOn[4]  = {0x27, 0xFF, 0x00, 0x00};
        I2C0_Write(ledOn, 4);   // RGB1: Red
        usleep(200*1000);
        uint8_t ledOff[4]  = {0x27, 0x00, 0x00, 0x00};
        I2C0_Write(ledOff, 4);
        usleep(200*1000);
    }

    while (g_carInfo.e_ST == eSt_BackRight) {
        uint8_t ledOn[4]  = {0x24, 0xFF, 0x00, 0x00};
        I2C0_Write(ledOn, 4);   // RGB0: Red
        usleep(200*1000);
        uint8_t ledOff[4]  = {0x24, 0x00, 0x00, 0x00};
        I2C0_Write(ledOff, 4);
        usleep(200*1000);
    }
}

void LedTask(void)
{
    while (1) {
        //LedCheck();
        LedCtrl();
        // usleep(50*1000);
    }
}


osThreadId_t LedTaskEntry(void)
{
    //{.name, .attr_bits, .cb_mem, .cb_size, .stack_mem, .stack_size, .priority, .tz_module, .reserved}
    osThreadAttr_t attr = {"LedTask", 0, NULL, 0, NULL, 1024*2, 24, 0, 0};
    osThreadId_t LedTaskId = osThreadNew((osThreadFunc_t)LedTask, NULL, &attr);

    if (LedTaskId == NULL) {
        printf("[LedTaskEntry] Failed to create %s!\n", attr.name);
    }
    return LedTaskId;
}
//SYS_RUN(LedTaskEntry);