package eu.intelix;

import org.knowm.xchart.*;
import org.knowm.xchart.XYSeries.*;
import org.knowm.xchart.style.Styler;

/**
 *
 * @author Daniel Dimov <danieldimov@gmail.com>
 */
public class PIDControl {

    private static final double REDUCTOR_RATIO = 60;
    
    private static final double TIME_STEP = 0.02; // 20 ms
    
    // this constant is the ratio between motor rpm and inverter frequency
    // example: ferquency 80 Hz --> 1200 rpm = 20 rps
    private static final double FREQ_2_RPM = 15;
    
    private static final double INVERTER_MAX_FREQ = 80;
    
    private static final int CHART_COUNT = 500;
    
    public static void main(String[] args) {
        
        double freq = 0;
        double reductorAngle = 0;
        double targetAngle = Math.PI;
        
        // play with these 3 constants:
        MiniPID pid2 = new MiniPID(30, 16, 380);
        pid2.setOutputLimits(INVERTER_MAX_FREQ);
        // maximum frequency change speed 1 Hz / 20 ms
        pid2.setOutputRampRate(1);
                
        double[] ra = new double[CHART_COUNT];
        double[] f = new double[CHART_COUNT];
        double[] x = new double[CHART_COUNT];
        
        for(int i = 0; i < CHART_COUNT; i++){
            double t = i * TIME_STEP;
            double rpm = freq * FREQ_2_RPM;
            reductorAngle += (rpm / 60) * 2 * Math.PI * TIME_STEP / REDUCTOR_RATIO;
            
            x[i] = t;
            ra[i] = Math.toDegrees(reductorAngle);
            f[i] = freq;
            
            double calculatedFreq = pid2.getOutput(reductorAngle, targetAngle);
            
            if(Math.abs(calculatedFreq) < 10) {
                if(Math.abs(calculatedFreq) < 2)
                    freq = 0;
                else
                    freq = 10 * Math.signum(calculatedFreq);
            } else {
                freq = Math.round(calculatedFreq);
            }
            
        }
        
        XYChart chart2 = new XYChartBuilder().width(1200).height(800).title("PID control simulation").xAxisTitle("Time [s]").build();
        chart2.getStyler().setDefaultSeriesRenderStyle(XYSeriesRenderStyle.Scatter);
        chart2.getStyler().setChartTitleVisible(true);
        chart2.getStyler().setLegendVisible(true);
        chart2.getStyler().setLegendPosition(Styler.LegendPosition.InsideNW);
        chart2.getStyler().setAxisTitlesVisible(true);
        chart2.getStyler().setXAxisDecimalPattern("0.0");
        XYSeries series1 = chart2.addSeries("Inverter frequency [Hz]", x, f);
        XYSeries series2 = chart2.addSeries("Reductor angle [deg]", x, ra);
        new SwingWrapper<>(chart2).displayChart();
        
    }
    
}