/*
 * Copyright 2020-2024 Yuntu Microelectronics co.,ltd
 * All rights reserved.
 *
 * YUNTU Confidential. This software is owned or controlled by YUNTU and may
 * only be used strictly in accordance with the applicable license terms. By expressly
 * accepting such terms or by downloading, installing, activating and/or otherwise
 * using the software, you are agreeing that you have read, and that you agree to
 * comply with and are bound by, such license terms. If you do not agree to be
 * bound by the applicable license terms, then you may not retain, install,
 * activate or otherwise use the software. The production use license in
 * Section 2.3 is expressly granted for this software.
 */
/*!
 * @file hw_spi.c
 */


#include "hw_hall.h"
#include "core_common.h"
#include "device_registers.h"
#include "interrupt_manager.h"
#include "motor_foc_lib.h"
#include "hw_config.h"
#include <math.h>
#include <stdint.h>
#include "hw_motor.h"

#define THETAGAIN        4
#define HALL1            ((eTMR2->IOSTS & eTMR_IOSTS_CH1IO_MASK)>> eTMR_IOSTS_CH1IO_SHIFT)        
#define HALL2            ((eTMR2->IOSTS & eTMR_IOSTS_CH0IO_MASK)>> eTMR_IOSTS_CH0IO_SHIFT)   
#define HALL3            ((eTMR1->IOSTS & eTMR_IOSTS_CH1IO_MASK)>> eTMR_IOSTS_CH1IO_SHIFT)     
#define TIMEBASE         107 
#define DEL_60_RADS_OMEGA    ((uint32_t)(22141289))    /* 10923*32767/16000 *10000 60°*/

#define D_ANGLEOFFSET      (910*28)
// #define D_ANGLEOFFSET      (0)
#define R_ANGLEOFFSET      (-910 * 2 - 455)

volatile uint8_t firstflg = 0;

volatile int16_t angle_offset = 0;

volatile uint8_t Motor_1_Hall = 0;

volatile uint8_t hall_state = 0;

volatile uint16_t hall_speed = 0;

volatile int16_t hall_theta = 0;

volatile uint16_t sector_speed[8] = {0};

volatile uint32_t delta_tick = 0;

volatile uint16_t last_tick = 0;

volatile uint16_t now_tick = 0;

volatile uint8_t MotorDirection = 1;

volatile uint8_t lastSector = 0;

volatile uint8_t NowSector = 0;

volatile uint16_t StopCnt = 0;

volatile uint8_t motorstop = 0;

// volatile uint8_t HallStopFlg = 0;
/* center const int16_t base_angles[8] = {-30, - 150, -90, 90, 30, 150};  */
const int16_t C_Angles[8] = {0, -5461, -27306, -16384, 16384, 5461, 27306, 0};
// /* Reverse angle base  */
// const uint16_t D_Angles[8] = {0, 10922+R_ANGLEOFFSET, 32767+R_ANGLEOFFSET, 21845+R_ANGLEOFFSET, 54612+R_ANGLEOFFSET, 65535+R_ANGLEOFFSET, 43690+R_ANGLEOFFSET, 0};
// /* //Direct angle base  */
// const uint16_t R_Angles[8] = {0, 10922+D_ANGLEOFFSET, 32767+D_ANGLEOFFSET, 21845+D_ANGLEOFFSET, 54612+D_ANGLEOFFSET, 65535+D_ANGLEOFFSET, 43690+D_ANGLEOFFSET, 0};

/* Reverse angle base  */
const uint16_t R_Angles[8] = {0, 10922+R_ANGLEOFFSET, 54613+R_ANGLEOFFSET, 0+R_ANGLEOFFSET, 32767+R_ANGLEOFFSET, 21845+R_ANGLEOFFSET, 43690+R_ANGLEOFFSET, 0};
/* //Direct angle base  */
const uint16_t D_Angles[8] = {0, 10922+D_ANGLEOFFSET, 54613+D_ANGLEOFFSET, 0+D_ANGLEOFFSET, 32767+D_ANGLEOFFSET, 21845+D_ANGLEOFFSET, 43690+D_ANGLEOFFSET, 0};


static void Hw_Hall_Speed_Calc(void);

static void Hw_Hall_Sector_Switch(void);

static void Hw_Hall_Sector_Switch(void)
{
    if(MotorDirection == 1)
    {
        switch(Motor_1_Hall)
        {
            case 1:
                if(hall_state != 5)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 2:
                if(hall_state != 3)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 3:
                if(hall_state != 1)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 4:
                if(hall_state != 6)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 5:
                if(hall_state != 4)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 6:
                if(hall_state != 2)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            default:
                /* 0 or 7 section */
                break;
        }
    }
    else if(MotorDirection == 0)
    {
        switch(Motor_1_Hall)
        {
            case 1:
                if(hall_state != 3)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 2:
                if(hall_state != 6)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 3:
                if(hall_state != 2)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 4:
                if(hall_state != 5)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 5:
                if(hall_state != 1)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            case 6:
                if(hall_state != 4)
                {
                    sector_speed[hall_state] = 1;
                }
                else
                {
                    Hw_Hall_Speed_Calc();
                }
                // 7. update globle state
                hall_state = Motor_1_Hall;
                // 6. update history sector
                break;
            default:
                /* 0 or 7 section */
                break;
        }
    }
    else
    {
        ;
    }
}
uint8_t TimeOvfCnt = 0;
static void Hw_Hall_Speed_Calc(void)
{
    delta_tick = TimeOvfCnt*65535 + now_tick - last_tick;
    TimeOvfCnt = 0;
    last_tick = now_tick;
    if(delta_tick > 1) // Avoid division by zero
    { 
        /* The rad/pwm at 360° electrical angle, omega */
        hall_speed = ((DEL_60_RADS_OMEGA / ((uint32_t)delta_tick)) * (100000/TIMEBASE) )>> 15;
        sector_speed[hall_state] = hall_speed;
        hall_speed = ((sector_speed[1] + sector_speed[2] + sector_speed[3] + sector_speed[4] + sector_speed[5] + sector_speed[6])/ 6);
    }

    if(MotorDirection == 1) /* Direct */
    {
        angle_offset = hall_theta - (int16_t)R_Angles[hall_state];
    }
    else if(MotorDirection == 0) /* Reverse */
    {
        angle_offset = hall_theta - (int16_t)D_Angles[hall_state];
    }
    else
    {
        ;
    }

    INT_SYS_DisableIRQGlobal();
    /* angle offset */
    hall_theta = hall_theta - (int16_t)(angle_offset / THETAGAIN);
    INT_SYS_EnableIRQGlobal();
}

void SpeedMonitor_Task(void) 
{
    uint8_t HalltaskPort1 = 0;

    HalltaskPort1 = (HALL1 + HALL2 + HALL3)& 0x1;

    NowSector = (uint8_t)(HalltaskPort1) << 2U;

    NowSector |= (uint8_t)(HALL2)<< 1U;

    NowSector |= (uint8_t)(HALL3);

    if(NowSector!=lastSector)
    {
        lastSector = NowSector;
        StopCnt = 0;
    }
    else
    {
        StopCnt++;
        if(StopCnt >= 8767)
        {
            motorstop = 1;
            hall_speed = 2;
            sector_speed[1] =  hall_speed;
            sector_speed[2] =  hall_speed;
            sector_speed[3] =  hall_speed;
            sector_speed[5] =  hall_speed;
            sector_speed[6] =  hall_speed;
            StopCnt = 0U;
        }
    }
}

uint8_t OvfCnt = 0;
void Hall_Ovf_IRQHandler()
{
    eTMR2->STS |= eTMR_STS_TOF_MASK;
    OvfCnt++;
}


void Hw_Hall_Senser(void)
{   
    uint8_t Hall1Port = 0;

    Hall1Port = (HALL1 + HALL2 + HALL3)& 0x1;

    now_tick =  eTMR2->CH[1].CVAL;

    TimeOvfCnt = OvfCnt;

    OvfCnt = 0;

    Motor_1_Hall = (uint8_t)(Hall1Port) << 2U;

    Motor_1_Hall |= (uint8_t)(HALL2)<< 1U;

    Motor_1_Hall |= (uint8_t)(HALL3);

    eTMR2->STS |= eTMR_STS_CH1F_MASK;

    /* update speed */
    if(Motor_1_Hall != hall_state)
    {
        if(MotorDirection == 1)
        {
            if(firstflg == 0)
            {
                firstflg = 1;
                last_tick = now_tick;
                hall_state = Motor_1_Hall;
                hall_theta = C_Angles[Motor_1_Hall];
                hall_speed = 2;
                
                for(uint8_t i = 1; i< 7; i++)
                {
                    sector_speed[i] = hall_speed;
                }
                /* The rad/pwm at 360° electrical angle, omega */
            }
            else if(firstflg <= 5)
            {
                firstflg++;
                last_tick = now_tick;
                hall_state = Motor_1_Hall;
                /* The rad/pwm at 360° electrical angle, omega */
                angle_offset = hall_theta - (int16_t)R_Angles[hall_state];
                INT_SYS_DisableIRQGlobal();
                /* angle offset */
                hall_theta = hall_theta - (int16_t)(angle_offset / THETAGAIN);
                INT_SYS_EnableIRQGlobal();
                // 6. update history sector
            }
            else
            {
                Hw_Hall_Sector_Switch();
            }
        }
        else if(MotorDirection == 0)
        {
            if(firstflg == 0)
            {
                firstflg = 1;
                last_tick = now_tick;
                hall_state = Motor_1_Hall;
                hall_theta = C_Angles[Motor_1_Hall];
                hall_speed = 2;
                
                for(uint8_t i = 1; i< 7; i++)
                {
                    sector_speed[i] = hall_speed;
                }
                /* The rad/pwm at 360° electrical angle, omega */
            }
            else if(firstflg <= 2)
            {
                firstflg++;
                last_tick = now_tick;
                hall_state = Motor_1_Hall;
                /* The rad/pwm at 360° electrical angle, omega */
                angle_offset = hall_theta - (int16_t)D_Angles[hall_state];
                INT_SYS_DisableIRQGlobal();
                /* angle offset */
                hall_theta = hall_theta - (int16_t)(angle_offset / THETAGAIN);
                INT_SYS_EnableIRQGlobal();
                // 6. update history sector
            }
            else
            {
                Hw_Hall_Sector_Switch();
            }
        }
        else
        {
            ;
        }
    }
}

void Hw_Hall_Init(void)
{
    INT_SYS_InstallHandler(eTMR2_Ch0_Ch1_IRQn, Hw_Hall_Senser, NULL);
}

/*******************************************************************************
* EOF
******************************************************************************/
 