using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class VelocitySetpointGeneration : MonoBehaviour
{

    public GetSumRed colorSensorValue;
    public float Dark = 0.4f;
    public float Bright = 1f;
    public float Threshold1 = 0.6f;
    public float Threshold2 = 0.8f;
    public float DistanceCenterToWheel = 0.07f;
    public float K = 0.01f;
    public float ForwardSpeed = 0.1f;

    public SteeringLineRobot Robot;

    private float meas;
    // Start is called before the first frame update
    private float omega;
 
    void Start()
    {
        
    }

    // Update is called once per frame
    void FixedUpdate()
    {
        float dimlessvalue;
        meas = colorSensorValue.TotalRed;
        if (meas < Dark)
        {
            dimlessvalue = -1f;
        }
        else if (meas < Threshold1)
        {
            dimlessvalue = -1* (Threshold1 - meas ) / (Threshold1 - Dark);
        }
        else if (meas < Threshold2)
        {
            dimlessvalue = 0f;
        }
        else if (meas < Bright)
        {
            dimlessvalue =  (meas - Threshold2) / (Bright - Threshold2);
        }
        else
        {
            dimlessvalue = 1f;
        }

        omega = K * dimlessvalue;
        float NettoForwardSpeed = ForwardSpeed; // Mathf.Max(ForwardSpeed - Mathf.Abs(omega * DistanceCenterToWheel), ForwardSpeed/4f);
        Robot.leftspeedsetpoint = NettoForwardSpeed - omega * DistanceCenterToWheel;
        Robot.rightspeedsetpoint = NettoForwardSpeed + omega * DistanceCenterToWheel;
    }
}
