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

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

#include <array>
#include <limits>
#include <cstdio>
#include <cstdint>

volatile uint32_t nextCommutationStep;
volatile uint32_t nextDrivePattern;
volatile uint32_t zcPolarity;
volatile uint32_t filteredTimeSinceCommutation;


std::array<uint32_t, 6> cwDriveTable {
    DRIVE_PATTERN_CW::STEP1,
    DRIVE_PATTERN_CW::STEP2,
    DRIVE_PATTERN_CW::STEP3,
    DRIVE_PATTERN_CW::STEP4,
    DRIVE_PATTERN_CW::STEP5,
    DRIVE_PATTERN_CW::STEP6
};
std::array<uint32_t, 6> ccwDriveTable{
    DRIVE_PATTERN_CCW::STEP1,
    DRIVE_PATTERN_CCW::STEP2,
    DRIVE_PATTERN_CCW::STEP3,
    DRIVE_PATTERN_CCW::STEP4,
    DRIVE_PATTERN_CCW::STEP5,
    DRIVE_PATTERN_CCW::STEP6
};
std::array<unsigned int, 24> startupDelays{
//    200, 150, 100, 80, 70, 65, 60, 55, 50, 45, 40, 35, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25
    200, 150, 100, 80, 70, 65, 60, 55, 50, 50, 50, 50, 50, 50, 50, 50, 50, 40, 40, 40, 40, 40, 40, 40
};
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;

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);
}

// 1sec 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);
}

void no_interrupt_handler (void) {};

volatile bool pwm_active=false;
void pwm_interrupt_handler(){
    pwm0::cfg_reg().cmp0ip=false;
    pwm_active=false;
}

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;
    qspi0::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;
    g_ext_interrupt_handlers[40] = pwm_interrupt_handler;
    // Priority must be set > 0 to trigger the interrupt.
    PLIC_set_priority(&g_plic, 40, 1);
    // Have to enable the interrupt both at the GPIO level, and at the PLIC level.
    PLIC_enable_interrupt (&g_plic, 40);
    // enable peripheral interrupt
    // GPIO_REG(GPIO_RISE_IE) |= (1 << BUTTON_0_OFFSET);

    // 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 Generates a delay used during startup
 *
 *  This functions is used to generate a delay during the startup procedure.
 *  The length of the delay equals delay * STARTUP_DELAY_MULTIPLIER microseconds.
 *  Since Timer/Counter1 is used in this function, it must never be called when
 *  sensorless operation is running.
 */
void StartupDelay(unsigned short delay){
#if 0
    delayUS(delay * STARTUP_DELAY_MULTIPLIER);
#else
    auto scaling_factor=0;
    unsigned d = delay * STARTUP_DELAY_MULTIPLIER;
    while(d/(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 = d/(1<<scaling_factor);
    pwm_active=true;
    pwm0::cfg_reg().enoneshot=true;
    do{
        asm("wfi");
        asm("nop");
    }while(pwm_active);
#endif
}

void StartMotor(void){
    auto& driveTable = ccw?ccwDriveTable:cwDriveTable;
    nextCommutationStep = 0;
    //Preposition.
    gpio0::port_reg() = driveTable[nextCommutationStep];
    printf("init\n");
    StartupDelay(STARTUP_LOCK_DELAY);
    nextCommutationStep++;
    nextDrivePattern = driveTable[nextCommutationStep];
    const size_t size=startupDelays.size();
    for (size_t i = 0; i < startupDelays.size()+100; i++){
        printf("step%d\n", i);
        gpio0::port_reg() = nextDrivePattern;
        auto index = i>=size?size-1:i;
        StartupDelay(startupDelays[index]);

        // switch ADC input
        // ADMUX = ADMUXTable[nextCommutationStep];

        // Use LSB of nextCommutationStep to determine zero crossing polarity.
        zcPolarity = nextCommutationStep & 0x01;

        nextCommutationStep++;
        if (nextCommutationStep >= 6){
            nextCommutationStep = 0;
        }
        nextDrivePattern = driveTable[nextCommutationStep];
    }
    // Switch to sensorless commutation.
    // Set filteredTimeSinceCommutation to the time to the next commutation.
    filteredTimeSinceCommutation = startupDelays[startupDelays.size() - 1] * (STARTUP_DELAY_MULTIPLIER  / 2);
}

int main() {
    platform_init();
	StartMotor();
	printf("done...");
	return 0;
}