//============================================================================
// Name        : riscv-bldc.cpp
// Author      : Eyck Jentzsch
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include "riscv-bldc.h"
#include "delay.h"
#include "bsp.h"
#include "plic/plic_driver.h"

#include <cstdio>
#include <cstdint>

#include "hifive1_io.h"

volatile uint32_t nextCommutationStep=0;

/* commutation blocks
 *     1   2   3   4   5   6
 * U   0   z   +1  +1  z   0
 * V   +1  +1  z   0   0   z
 * W   z   0   0   z   +1  +1
 */

std::array<uint32_t, 6> driveTable { //! Drive pattern for commutation, CW rotation
            ((1 << VH) | (1 << UL)), //1
            ((1 << VH) | (1 << WL)), //2
            ((1 << UH) | (1 << WL)), //3
            ((1 << UH) | (1 << VL)), //4
            ((1 << WH) | (1 << VL)), //5
            ((1 << WH) | (1 << UL))  //6
};

std::array<uint32_t, 6> senseTable { //! channels to sense during the applied pattern
    SENSW_N, //1
    SENSU_P, //2
    SENSV_N, //3
    SENSW_P, //4
    SENSU_N, //5
    SENSV_P  //6
};

bool ccw=false;

typedef void (*function_ptr_t) (void);
//! Instance data for the PLIC.
plic_instance_t g_plic;
std::array<function_ptr_t,PLIC_NUM_INTERRUPTS>  g_ext_interrupt_handlers;
/*! \brief external interrupt handler
 *
 * routes the peripheral interrupts to the the respective handler
 *
 */
extern "C" void handle_m_ext_interrupt() {
    plic_source int_num  = PLIC_claim_interrupt(&g_plic);
    if ((int_num >=1 ) && (int_num < PLIC_NUM_INTERRUPTS))
      g_ext_interrupt_handlers[int_num]();
    else
      exit(1 + (uintptr_t) int_num);
    PLIC_complete_interrupt(&g_plic, int_num);
}
/*! \brief  mtime interval interrupt
 *
 */
extern "C" void handle_m_time_interrupt(){
    clear_csr(mie, MIP_MTIP);
    // Reset the timer for 3s in the future.
    // This also clears the existing timer interrupt.
    volatile uint64_t * mtime       = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIME);
    volatile uint64_t * mtimecmp    = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
    uint64_t now = *mtime;
    uint64_t then = now + RTC_FREQ;
    *mtimecmp = then;
    // Re-enable the timer interrupt.
    set_csr(mie, MIP_MTIP);
}
/*! \brief dummy interrupt handler
 *
 */
void no_interrupt_handler (void) {};
/*! \brief configure the per-interrupt handler
 *
 */
void configure_irq(size_t irq_num, function_ptr_t handler, unsigned char prio=1) {
    g_ext_interrupt_handlers[irq_num] = handler;
    // Priority must be set > 0 to trigger the interrupt.
    PLIC_set_priority(&g_plic, irq_num, prio);
    // Have to enable the interrupt both at the GPIO level, and at the PLIC level.
    PLIC_enable_interrupt(&g_plic, irq_num);
}
/*!\brief initializes platform
 *
 */
void platform_init(){
    // configure clocks
    PRCI_use_hfxosc(1); // is equivalent to
    // init UART0 at 115200 baud
    auto baud_rate=115200;
    gpio0::output_en_reg()=0xffffffff;
    gpio0::iof_sel_reg()&=~IOF0_UART0_MASK;
    gpio0::iof_en_reg()|= IOF0_UART0_MASK;
    uart0::div_reg()=get_cpu_freq() / baud_rate - 1;
    uart0::txctrl_reg().txen=1;
    // init SPI
    gpio0::iof_sel_reg()&=~IOF0_SPI1_MASK;
    gpio0::iof_en_reg()|= IOF0_SPI1_MASK;
    qspi1::sckdiv_reg() = 8;

    F_CPU=PRCI_measure_mcycle_freq(20, RTC_FREQ);
    printf("core freq at %d Hz\n", F_CPU);
    // initialie interupt & trap handling
    write_csr(mtvec, &trap_entry);
    if (read_csr(misa) & (1 << ('F' - 'A'))) { // if F extension is present
      write_csr(mstatus, MSTATUS_FS); // allow FPU instructions without trapping
      write_csr(fcsr, 0); // initialize rounding mode, undefined at reset
    }

    PLIC_init(&g_plic, PLIC_CTRL_ADDR, PLIC_NUM_INTERRUPTS, PLIC_NUM_PRIORITIES);
    // Disable the machine & timer interrupts until setup is done.
    clear_csr(mie, MIP_MEIP);
    clear_csr(mie, MIP_MTIP);
    for (auto& h:g_ext_interrupt_handlers) h=no_interrupt_handler;
    configure_irq(40, pwm0::pwm_interrupt_handler);
    configure_irq(6, qspi1::spi_rx_interrupt_handler);
    // Set the machine timer to go off in 1 second.
    volatile uint64_t * mtime       = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIME);
    volatile uint64_t * mtimecmp    = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
    uint64_t now = *mtime;
    uint64_t then = now + RTC_FREQ;
    *mtimecmp = then;
    // Enable the Machine-External bit in MIE
    set_csr(mie, MIP_MEIP);
    // Enable the Machine-Timer bit in MIE
    set_csr(mie, MIP_MTIP);
    // Enable interrupts in general.
    set_csr(mstatus, MSTATUS_MIE);
}
/*! \brief reads adc channel and returns measured value
 *
 */
unsigned read_adc(unsigned channel){
    std::array<uint8_t, 3> bytes{
                uint8_t(0x06 | (channel>>2 & 0x1)), /* start bit, single ended measurement, channel[2] */
                uint8_t((channel&0x3)<<6),          /* channel[1:0], fill*/
                0x0                                 /* fill */
    };
    // set CS of target
    qspi1::csid_reg()=0;
    qspi1::transfer(bytes);
    return (bytes[1]&0xf)*256+bytes[2];
}
/*! \brief waits for zero crossing and measures time until
 *
 */
unsigned short measured_zc_time(unsigned short max_delay){
    long delay_us = max_delay;
    auto scaling_factor=0;
    while(delay_us/(1<<scaling_factor) > std::numeric_limits<unsigned short>::max()){
        scaling_factor++;
    }
    pwm0::cfg_reg()=0;
    pwm0::count_reg()=0;
    pwm0::cfg_reg().scale = 4+scaling_factor; // divide by 16 so we get 1us per pwm clock
    pwm0::cmp0_reg().cmp0 = delay_us/(1<<scaling_factor);
    pwm0::set_active();
    pwm0::cfg_reg().enoneshot=true;
    uint32_t channel=senseTable[nextCommutationStep]&0x3;
    bool zc_neg = senseTable[nextCommutationStep]>3;
    uint32_t adc_res=0;
    do{
        adc_res=read_adc(channel);
        if((zc_neg && adc_res<2048) || (!zc_neg && adc_res>2047))
            break;
    } while(pwm0::is_active());
    uint32_t sreg = pwm0::s_reg();
    pwm0::cfg_reg().enoneshot=false;
    return sreg*(1<<scaling_factor);
}
/*! \brief calculates the next commutation step
 *
 */
inline void next_commutation_step(void) {
    if (ccw) {
        nextCommutationStep = nextCommutationStep == 0? 5 : nextCommutationStep-1;
    } else {
        nextCommutationStep = nextCommutationStep == 5? 0 : nextCommutationStep+1;
    }
}
/*! \brief write the drive pattern to gpio
 *
 */
void setDrivePattern(uint32_t nextDrivePattern) {
    gpio0::port_reg() = (gpio0::port_reg() & ~DRIVE_MASK & 0x00ffffff) | nextDrivePattern | nextCommutationStep << 24;
}
/*! \brief open-loop commutation to start the motor
 *
 */
void start_open_loop(void){
    auto delay = 30120U;
    std::array<double, 2> multiplier={0.83, 1.0};
    nextCommutationStep = 0;
    //Preposition.
    gpio0::port_reg() = (gpio0::port_reg() & ~DRIVE_MASK) | driveTable[nextCommutationStep];
    //fixed_delay(STARTUP_LOCK_DELAY);
    pwm0::oneshot_delay(STARTUP_DELAY);
    next_commutation_step();
    auto nextDrivePattern = driveTable[nextCommutationStep];
    for (size_t i = 0; i < 12; i++){
        setDrivePattern(nextDrivePattern);
        auto channel=senseTable[nextCommutationStep]&0x3;
        auto zcPolRise = senseTable[nextCommutationStep]<4;
        auto bemf_0=read_adc(channel);
        delay*=multiplier[(i/6)%multiplier.size()];
        pwm0::oneshot_delay(delay);
        auto bemf_1=read_adc(channel);
        auto bemf = bemf_1>bemf_0?bemf_1-bemf_0:bemf_0-bemf_1;
        next_commutation_step();
        nextDrivePattern = driveTable[nextCommutationStep];
    }
}
/*! \brief closed-loop commutation to run the motor
 *
 */
void run_closed_loop(void){
    auto count=0;
    auto zc_delay=0U;
    auto tmp=0U;
    auto nextDrivePattern = driveTable[nextCommutationStep];
    for(;;){
        setDrivePattern(nextDrivePattern);
        zc_delay=measured_zc_time(50000);
        next_commutation_step();
        nextDrivePattern = driveTable[nextCommutationStep];
    }
}
/*! \brief main function
 *
 */
int main() {
    platform_init();
    printf("Starting motor\n");
	start_open_loop();
    printf("done...\n");
    // Switch to sensor-less closed-loop commutation.
	run_closed_loop();
	return 0;
}