#include <exec/types.h>
#include <exec/memory.h>
#include <dos/rdargs.h>

#include <proto/exec.h>
#include <proto/dos.h>
#include <proto/intuition.h>
#include <proto/expansion.h>
#include <hardware/cia.h>

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>

#include "tfconfig.h"

//struct Library   *ExpansionBase = NULL;
struct ConfigDev *cd = NULL;

#define TEMPLATE "SPEED/K/N/A,CRYSTAL/K/N,PHASE/K/N"
#define OPT_SPEED 0
#define OPT_CRYSTAL 1
#define OPT_PHASE 2
#define OPT_COUNT 3


#define MHZ 1E6
#define DEFAULT_CRYSTAL_SPEED 25*MHZ

#define BUF_SIZE 256

float speed_multipliers[] = {
    4.0,
    5.0,
    6.0,
    6.25,
    6.3333333,
    8.0,
    8.3333333,
    10.0,
    12.0
};

float get_actual_cpu_speed(float multiplier, float crystal_speed)
{
    // compute the CPU speed from the multiplier selection
    return (multiplier * crystal_speed) / 2.0f;
}

UBYTE get_multiplier_setting(float desired_speed, float crystal_speed)
{
    // default lowest setting
    UBYTE multiplier_setting = 0;
    size_t speed_multipliers_len = sizeof(speed_multipliers) / sizeof(float);

    for (UBYTE i=0; i < speed_multipliers_len; i++)
    {
        float actual_speed = get_actual_cpu_speed(speed_multipliers[i], crystal_speed);

        if (actual_speed > desired_speed) break;

        multiplier_setting = i;
    }

    return multiplier_setting;
}

volatile struct CIA *ciaa = (volatile struct CIA *)0xbfe001;

__section(code_c) int main (void)
{
    int  cpu_speed = -1;
    int  cpu_phase = 0;

    int  crystal_speed = DEFAULT_CRYSTAL_SPEED;


    char template[BUF_SIZE];
    strcpy(template, TEMPLATE);
    LONG result[OPT_COUNT] = {0, 0, 0};

    struct RDArgs *rd = ReadArgs(template, result, NULL);

    printf("TerribleFire Cpu Speed Tool\n");
    printf("(C) 2019 S.J.Leary\n");

    if (rd != NULL)
    {
        LONG* speed_opt = (LONG *)result[OPT_SPEED];
        LONG* crystal_option = (LONG *)result[OPT_CRYSTAL];
        LONG* phase_option = (LONG *)result[OPT_PHASE];

        cpu_speed = (int) speed_opt[0] * MHZ;

        if ((crystal_option != NULL) && (int)crystal_option[0] > 0)
        {
            crystal_speed = (int) crystal_option[0] * MHZ;
            printf("Using Non-Default Crystal: %i\n", crystal_speed);
        }

        if ((phase_option != NULL) && (int)phase_option[0] > 0)
        {
            cpu_phase = (int) phase_option[0] & 0xF;
            printf("Using Non-Default Phase: %i\n", cpu_phase);
        }

        FreeArgs(rd);
    }

    UBYTE multiplier_setting = get_multiplier_setting(cpu_speed, crystal_speed);

    if (cpu_speed > 0)
    {
        printf("Requesting speed: %3.2f MHZ\n", ((float)cpu_speed / MHZ));
        printf("Multiplier setting: %u\n", multiplier_setting);
        printf("Nearest CPU Speed: %3.2f MHZ\n", get_actual_cpu_speed(speed_multipliers[multiplier_setting], crystal_speed) / MHZ);
    }

    if((ExpansionBase=(APTR)OpenLibrary("expansion.library",0L))==NULL)
    {
        printf("FATAL: Unable to open expansion.library\n");
        exit(RETURN_FAIL);
    }

    cd = (struct ConfigDev*)FindConfigDev(NULL, 0x13D8, 0x82);


    if (cd)
    {

        printf("TF360/TF1260 Clock Control Device Found.\n");
        printf("Serial: %lu\n", cd->cd_Rom.er_SerialNumber);

        struct TFConfig *tfconfig_device = (struct TFConfig*) (cd->cd_BoardAddr);

        Disable();

        if (cpu_speed > 0)
        {
            multiplier_setting |= (cpu_phase << 4);
            tfconfig_device->TF_Speed = (UBYTE) multiplier_setting;

            for (int i =0; i < 3000; i++)
            {
                (volatile) ciaa->ciapra;
            }

        }

        Enable();

        cpu_speed = (int) (0x0F & tfconfig_device->TF_Status);
        cpu_phase = (int) (0xF0 & tfconfig_device->TF_Status) >> 4;

        printf("CPU Speed: %3.2f MHZ\n", get_actual_cpu_speed(speed_multipliers[cpu_speed], crystal_speed)/MHZ);
        printf("Clock Phase: %i\n", cpu_phase);
    }
    else
    {
        printf("TF360/TF1260 Clock Control Device Not Found.\n");
    }

    CloseLibrary((APTR)ExpansionBase);


    return 0;
}