package com.qualcomm.ftcrobotcontroller;

import com.qualcomm.ftccommon.DbgLog;
import com.qualcomm.ftcrobotcontroller.control.Control;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;

import org.json.simple.JSONArray;
import org.json.simple.JSONObject;
import org.json.simple.parser.JSONParser;
import org.json.simple.parser.ParseException;

import java.io.IOException;
import java.nio.charset.Charset;
import java.util.HashMap;
import java.util.Map;


public abstract class RMOpMode extends OpMode {

    protected Map<String, Motor> motorMap = new HashMap<String, Motor>();
    protected Map<String, rServo> servoMap =  new HashMap<String, rServo>();
    protected Control control;

    @Override
    public void init() {
        telemetry.addData("init", "init start");
        try {
            this.configureHardware(this.setConfigurationPath());
        } catch (IOException e) {
            e.printStackTrace();
            DbgLog.error(e.getMessage());
        } catch (ParseException e) {
            e.printStackTrace();
            DbgLog.error(e.getMessage());
        }
        this.control = new Control(gamepad1, gamepad2);
        telemetry.addData("init","done");
    }

    @Override
    public void loop() {
        telemetry.addData("loop","loop start");
        this.updateInput();
        this.calculate();
        this.updateHardware();
        telemetry.addData("loop","loop finish");
    }

    protected void updateInput() {
        telemetry.addData("updateInput", "updateInput start");
        control.update(gamepad1, gamepad2);
        telemetry.addData("updateInput","updateInput finish");
    }

    protected abstract void calculate();

    protected void updateHardware() {
        telemetry.addData("updateHardware","updateHardware start");
        for (Motor m : motorMap.values()) {
            m.updateCurrentPower();
            m.setCurrentPower();
        }
        for (rServo s : servoMap.values()) {
            s.updateCurrentPosition();
            s.setPosition();
        }
        telemetry.addData("updateHardware","updateHardware finish");
    }

    protected abstract String setConfigurationPath();

    protected void configureHardware(String pathName) throws IOException, ParseException {
        telemetry.addData("configureHardware",pathName);
        telemetry.addData("configureHardware","configureHardware start");
        String configSource = readFile(pathName, Charset.defaultCharset());
        JSONParser jsonParser = new JSONParser();
        JSONObject jsonFile = (JSONObject) jsonParser.parse(configSource);
        JSONArray jsonMotors = (JSONArray) jsonFile.get("motors");
        JSONArray jsonServos = (JSONArray) jsonFile.get("servos");
        this.configureMotors(jsonMotors);
        this.configureServos(jsonServos);
        //Todo add methods for configuring and sensors
        telemetry.addData("configureHardware","configureHardware finish");
    }

    private static String readFile(String path, Charset encoding) throws IOException {
        /*byte[] encoded = Files.readAllBytes(Paths.get(path));
        return new String(encoded, encoding);
        */
        return path;
    }

    private void configureMotors(JSONArray JSONMotors) {
        telemetry.addData("configureMotors","configureMotors start");
        for (Object mObj : JSONMotors) {
            JSONObject mJSON = (JSONObject) mObj;
            String motorName = (String) mJSON.get("name");
            double minPower = (Double) mJSON.get("minPower");
            double maxPower = (Double) mJSON.get("maxPower");
            DcMotor dcParent = hardwareMap.dcMotor.get(motorName);
            DcMotor.Direction d = stringToMotorDirection((String) mJSON.get("direction"));
            Motor m = new Motor(dcParent, d, minPower, maxPower);
            motorMap.put(motorName, m);
        }
        telemetry.addData("configureMotors","configureMotors finish");
    }

    private void configureServos(JSONArray JSONServos) {
        telemetry.addData("configureServos","configureServos start");
        for (Object sObj : JSONServos) {
            JSONObject sJSON = (JSONObject) sObj;
            String servoName = (String) sJSON.get("name");
            double minPosition = (Double) sJSON.get("minPosition");
            double maxPosition = (Double) sJSON.get("maxPosition");
            Servo sParent = hardwareMap.servo.get(servoName);
            Servo.Direction d = stringToServoDirection((String) sJSON.get("direction"));
            rServo s = new rServo(sParent, d, minPosition, maxPosition);
            servoMap.put(servoName, s);
        }
        telemetry.addData("configureServos","configureServos finish");
    }

    private DcMotor.Direction stringToMotorDirection(String stringD) { //ToDo check if valueOf works as expected
        telemetry.addData("stringToMotorDirection","stmd start");
        if (stringD == "FORWARD") {
            return DcMotor.Direction.FORWARD;
        } else if (stringD == "REVERSE") {
            return DcMotor.Direction.REVERSE;
        } else {
            return DcMotor.Direction.valueOf(stringD);
        }
    }

    private Servo.Direction stringToServoDirection(String stringD) {
        telemetry.addData("stringToServoDirection","stsd start");
        if (stringD == "FORWARD") {
            return Servo.Direction.FORWARD;
        } else if (stringD == "REVERSE") {
            return Servo.Direction.REVERSE;
        } else {
            return Servo.Direction.valueOf(stringD);
        }
    }
}
@@ -232,16 +232,74 @@
 cMotor;%0A
+import com.qualcomm.robotcore.hardware.DcMotorController;%0A
 import c
@@ -4955,74 +4955,8 @@
 ted%0A
-        telemetry.addData(%22stringToMotorDirection%22,%22stmd start%22);%0A
     
@@ -4979,32 +4979,32 @@
 == %22FORWARD%22) %7B%0A
+
             retu
@@ -5284,74 +5284,8 @@
 ) %7B%0A
-        telemetry.addData(%22stringToServoDirection%22,%22stsd start%22);%0A
     
@@ -5456,32 +5456,32 @@
        %7D else %7B%0A
-
             retu
@@ -5532,10 +5532,545 @@
  %7D%0A    %7D
+%0A%0A    private DcMotorController.RunMode stringToRunMode(String stringR) %7B%0A        if (stringR.equals(%22RUN_TO_POSITION%22)) %7B%0A            return DcMotorController.RunMode.RUN_TO_POSITION;%0A        %7D else if (stringR.equals(%22RUN_WITHOUT_ENCODERS%22)) %7B%0A            return DcMotorController.RunMode.RUN_WITHOUT_ENCODERS;%0A        %7D else if (stringR.equals(%22RUN_USING_ENCODERS%22)) %7B%0A            return DcMotorController.RunMode.RUN_USING_ENCODERS;%0A        %7D else %7B%0A            return DcMotorController.RunMode.RUN_WITHOUT_ENCODERS%0A        %7D%0A    %7D
 %0A%7D

Add stringToRunMode