diff --git a/legacy/builder/add_additional_entries_to_context.go b/legacy/builder/add_additional_entries_to_context.go
index 775b6cdc63e..23d4a296146 100644
--- a/legacy/builder/add_additional_entries_to_context.go
+++ b/legacy/builder/add_additional_entries_to_context.go
@@ -63,8 +63,6 @@ func (*AddAdditionalEntriesToContext) Run(ctx *types.Context) error {
 		ctx.WarningsLevel = DEFAULT_WARNINGS_LEVEL
 	}
 
-	ctx.CollectedSourceFiles = &types.UniqueSourceFileQueue{}
-
 	ctx.LibrariesResolutionResults = map[string]types.LibraryResolutionResult{}
 	ctx.HardwareRewriteResults = map[*cores.PlatformRelease][]types.PlatforKeyRewrite{}
 
diff --git a/legacy/builder/container_find_includes.go b/legacy/builder/container_find_includes.go
index c8c5325eac9..11eb83a7a78 100644
--- a/legacy/builder/container_find_includes.go
+++ b/legacy/builder/container_find_includes.go
@@ -104,6 +104,7 @@ import (
 	"github.com/arduino/arduino-cli/legacy/builder/utils"
 	"github.com/arduino/go-paths-helper"
 	"github.com/pkg/errors"
+	"github.com/sirupsen/logrus"
 )
 
 type ContainerFindIncludes struct{}
@@ -121,59 +122,109 @@ func (s *ContainerFindIncludes) Run(ctx *types.Context) error {
 }
 
 func (s *ContainerFindIncludes) findIncludes(ctx *types.Context) error {
-	cachePath := ctx.BuildPath.Join("includes.cache")
-	cache := readCache(cachePath)
+	finder := NewCppIncludesFinder(ctx, ctx.BuildPath.Join("includes.cache"))
+	finder.UseIncludeDir(ctx.BuildProperties.GetPath("build.core.path"))
+	if variantPath := ctx.BuildProperties.GetPath("build.variant.path"); variantPath != nil {
+		finder.UseIncludeDir(variantPath)
+	}
+	finder.AddSourceFile(ctx.SketchBuildPath, ctx.SketchBuildPath, paths.New(ctx.Sketch.MainFile.Base()+".cpp"))
+	finder.AddSourceDir(ctx.SketchBuildPath, ctx.SketchBuildPath, false /* recurse */)
+	if srcSubfolderPath := ctx.SketchBuildPath.Join("src"); srcSubfolderPath.IsDir() {
+		finder.AddSourceDir(srcSubfolderPath, ctx.SketchBuildPath, true /* recurse */)
+	}
 
-	appendIncludeFolder(ctx, cache, nil, "", ctx.BuildProperties.GetPath("build.core.path"))
-	if ctx.BuildProperties.Get("build.variant.path") != "" {
-		appendIncludeFolder(ctx, cache, nil, "", ctx.BuildProperties.GetPath("build.variant.path"))
+	if err := finder.DetectLibraries(); err != nil {
+		return err
 	}
 
-	sketch := ctx.Sketch
-	mergedfile, err := types.MakeSourceFile(ctx, sketch, paths.New(sketch.MainFile.Base()+".cpp"))
-	if err != nil {
+	ctx.ImportedLibraries.Add(finder.LibrariesFound...)
+	ctx.IncludeFolders.AddAllMissing(finder.IncludeDirsFound)
+	if err := runCommand(ctx, &FailIfImportedLibraryIsWrong{}); err != nil {
 		return errors.WithStack(err)
 	}
-	ctx.CollectedSourceFiles.Push(mergedfile)
 
-	sourceFilePaths := ctx.CollectedSourceFiles
-	queueSourceFilesFromFolder(ctx, sourceFilePaths, sketch, ctx.SketchBuildPath, false /* recurse */)
-	srcSubfolderPath := ctx.SketchBuildPath.Join("src")
-	if srcSubfolderPath.IsDir() {
-		queueSourceFilesFromFolder(ctx, sourceFilePaths, sketch, srcSubfolderPath, true /* recurse */)
+	return nil
+}
+
+// CppIncludesFinder implements an algorithm to automatically detect
+// libraries used in a sketch and a way to cache this result for
+// increasing detection speed on already processed sketches.
+type CppIncludesFinder struct {
+	LibrariesFound   libraries.List
+	IncludeDirsFound paths.PathList
+	ctx              *types.Context
+	cache            *includeCache
+	queue            *UniqueSourceFileQueue
+	log              *logrus.Entry
+}
+
+// NewCppIncludesFinder create a new include
+func NewCppIncludesFinder(ctx *types.Context, cachePath *paths.Path) *CppIncludesFinder {
+	return &CppIncludesFinder{
+		ctx:   ctx,
+		cache: loadCacheFrom(cachePath),
+		queue: &UniqueSourceFileQueue{},
+		log:   logrus.WithField("task", "DetectingLibraries"),
+	}
+}
+
+// DetectLibraries runs a library detection algorithm
+func (f *CppIncludesFinder) DetectLibraries() error {
+	for _, includeDir := range f.IncludeDirsFound {
+		f.log.Debugf("Using include directory: %s", includeDir)
+		f.cache.AddAndCheckEntry(nil, "", includeDir)
 	}
 
-	for !sourceFilePaths.Empty() {
-		err := findIncludesUntilDone(ctx, cache, sourceFilePaths.Pop())
-		if err != nil {
-			cachePath.Remove()
+	for !f.queue.Empty() {
+		if err := f.findIncludesUntilDone(f.queue.Pop()); err != nil {
+			f.cache.Remove()
 			return errors.WithStack(err)
 		}
 	}
 
 	// Finalize the cache
-	cache.ExpectEnd()
-	err = writeCache(cache, cachePath)
-	if err != nil {
+	f.cache.ExpectEnd()
+	if err := f.cache.WriteToFile(); err != nil {
 		return errors.WithStack(err)
 	}
+	return nil
+}
 
-	err = runCommand(ctx, &FailIfImportedLibraryIsWrong{})
-	if err != nil {
+// UseIncludeDir adds an include directory to the current library discovery
+func (f *CppIncludesFinder) UseIncludeDir(includeDir *paths.Path) {
+	f.IncludeDirsFound.Add(includeDir)
+}
+
+// AddSourceFile adds a source file to be examined to look for library imports
+func (f *CppIncludesFinder) AddSourceFile(sourceRoot, buildRoot, srcPath *paths.Path) error {
+	if file, err := MakeSourceFile(sourceRoot, buildRoot, srcPath); err == nil {
+		f.log.Debugf("Queueing source file: %s", file)
+		f.queue.Push(file)
+	} else {
 		return errors.WithStack(err)
 	}
-
 	return nil
 }
 
-// Append the given folder to the include path and match or append it to
-// the cache. sourceFilePath and include indicate the source of this
-// include (e.g. what #include line in what file it was resolved from)
-// and should be the empty string for the default include folders, like
-// the core or variant.
-func appendIncludeFolder(ctx *types.Context, cache *includeCache, sourceFilePath *paths.Path, include string, folder *paths.Path) {
-	ctx.IncludeFolders = append(ctx.IncludeFolders, folder)
-	cache.ExpectEntry(sourceFilePath, include, folder)
+// AddSourceDir adds a directory of source file to be examined to look for library imports
+func (f *CppIncludesFinder) AddSourceDir(srcDir, buildDir *paths.Path, recurse bool) error {
+	extensions := func(ext string) bool { return ADDITIONAL_FILE_VALID_EXTENSIONS_NO_HEADERS[ext] }
+	f.log.Debugf("  Queueing source files from %s (recurse %v)", srcDir, recurse)
+	filePaths, err := utils.FindFilesInFolder(srcDir.String(), extensions, recurse)
+	if err != nil {
+		return errors.WithStack(err)
+	}
+
+	for _, filePath := range filePaths {
+		sourceFile, err := MakeSourceFile(srcDir, buildDir, paths.New(filePath))
+		if err != nil {
+			return errors.WithStack(err)
+		}
+		f.log.Debugf("    Queuing %s", sourceFile)
+		f.queue.Push(sourceFile)
+	}
+
+	return nil
 }
 
 func runCommand(ctx *types.Context, command types.Command) error {
@@ -192,8 +243,13 @@ type includeCacheEntry struct {
 }
 
 func (entry *includeCacheEntry) String() string {
-	return fmt.Sprintf("SourceFile: %s; Include: %s; IncludePath: %s",
-		entry.Sourcefile, entry.Include, entry.Includepath)
+	if entry.Sourcefile == nil {
+		return fmt.Sprintf("Include path added: %s", entry.Includepath)
+	}
+	if entry.Include == "" {
+		return fmt.Sprintf("%s successfully compiled", entry.Sourcefile)
+	}
+	return fmt.Sprintf("%s requires %s: include path added %s", entry.Sourcefile, entry.Include, entry.Includepath)
 }
 
 func (entry *includeCacheEntry) Equals(other *includeCacheEntry) bool {
@@ -203,102 +259,118 @@ func (entry *includeCacheEntry) Equals(other *includeCacheEntry) bool {
 type includeCache struct {
 	// Are the cache contents valid so far?
 	valid bool
+
+	// The file where to save the cache
+	cacheFilePath *paths.Path
+
 	// Index into entries of the next entry to be processed. Unused
 	// when the cache is invalid.
 	next    int
 	entries []*includeCacheEntry
 }
 
-// Return the next cache entry. Should only be called when the cache is
-// valid and a next entry is available (the latter can be checked with
-// ExpectFile). Does not advance the cache.
-func (cache *includeCache) Next() *includeCacheEntry {
+// Peek returns the next cache entry if the cache is valid and the next
+// entry exists, otherwise it returns nil. Does not advance the cache.
+func (cache *includeCache) Peek() *includeCacheEntry {
+	if !cache.valid || cache.next >= len(cache.entries) {
+		return nil
+	}
 	return cache.entries[cache.next]
 }
 
-// Check that the next cache entry is about the given file. If it is
-// not, or no entry is available, the cache is invalidated. Does not
-// advance the cache.
+// Invalidate invalidates the cache.
+func (cache *includeCache) Invalidate() {
+	cache.valid = false
+	cache.entries = cache.entries[:cache.next]
+}
+
+// ExpectFile check that the next cache entry is about the given file.
+// If it is not, or no entry is available, the cache is invalidated.
+// Does not advance the cache.
 func (cache *includeCache) ExpectFile(sourcefile *paths.Path) {
-	if cache.valid && (cache.next >= len(cache.entries) || !cache.Next().Sourcefile.EqualsTo(sourcefile)) {
-		cache.valid = false
-		cache.entries = cache.entries[:cache.next]
+	if next := cache.Peek(); next == nil || !next.Sourcefile.EqualsTo(sourcefile) {
+		cache.Invalidate()
 	}
 }
 
-// Check that the next entry matches the given values. If so, advance
-// the cache. If not, the cache is invalidated. If the cache is
-// invalidated, or was already invalid, an entry with the given values
-// is appended.
-func (cache *includeCache) ExpectEntry(sourcefile *paths.Path, include string, librarypath *paths.Path) {
-	entry := &includeCacheEntry{Sourcefile: sourcefile, Include: include, Includepath: librarypath}
-	if cache.valid {
-		if cache.next < len(cache.entries) && cache.Next().Equals(entry) {
-			cache.next++
-		} else {
-			cache.valid = false
-			cache.entries = cache.entries[:cache.next]
-		}
+// AddAndCheckEntry check that the next entry matches the given values.
+// If so, advance the cache. If not, the cache is invalidated. If the
+// cache is invalidated, or was already invalid, an entry with the given
+// values is appended.
+func (cache *includeCache) AddAndCheckEntry(sourcefile *paths.Path, include string, librarypath *paths.Path) {
+	expected := &includeCacheEntry{Sourcefile: sourcefile, Include: include, Includepath: librarypath}
+	if next := cache.Peek(); next == nil || !next.Equals(expected) {
+		cache.Invalidate()
+	} else {
+		cache.next++
 	}
 
 	if !cache.valid {
-		cache.entries = append(cache.entries, entry)
+		cache.entries = append(cache.entries, expected)
+		cache.next++
 	}
 }
 
-// Check that the cache is completely consumed. If not, the cache is
-// invalidated.
+// ExpectEnd check that the cache is completely consumed. If not, the
+// cache is invalidated.
 func (cache *includeCache) ExpectEnd() {
 	if cache.valid && cache.next < len(cache.entries) {
-		cache.valid = false
-		cache.entries = cache.entries[:cache.next]
+		cache.Invalidate()
 	}
 }
 
-// Read the cache from the given file
-func readCache(path *paths.Path) *includeCache {
-	bytes, err := path.ReadFile()
-	if err != nil {
-		// Return an empty, invalid cache
-		return &includeCache{}
+// Remove removes the cache file from disk.
+func (cache *includeCache) Remove() error {
+	return cache.cacheFilePath.Remove()
+}
+
+// loadCacheFrom read the cache from the given file
+func loadCacheFrom(path *paths.Path) *includeCache {
+	result := &includeCache{
+		cacheFilePath: path,
+		valid:         false,
 	}
-	result := &includeCache{}
-	err = json.Unmarshal(bytes, &result.entries)
-	if err != nil {
-		// Return an empty, invalid cache
-		return &includeCache{}
+	if bytes, err := path.ReadFile(); err != nil {
+		return result
+	} else if err = json.Unmarshal(bytes, &result.entries); err != nil {
+		return result
 	}
 	result.valid = true
 	return result
 }
 
-// Write the given cache to the given file if it is invalidated. If the
+// WriteToFile the cache file if it is invalidated. If the
 // cache is still valid, just update the timestamps of the file.
-func writeCache(cache *includeCache, path *paths.Path) error {
+func (cache *includeCache) WriteToFile() error {
 	// If the cache was still valid all the way, just touch its file
 	// (in case any source file changed without influencing the
 	// includes). If it was invalidated, overwrite the cache with
 	// the new contents.
 	if cache.valid {
-		path.Chtimes(time.Now(), time.Now())
+		cache.cacheFilePath.Chtimes(time.Now(), time.Now())
 	} else {
-		bytes, err := json.MarshalIndent(cache.entries, "", "  ")
-		if err != nil {
+		if bytes, err := json.MarshalIndent(cache.entries, "", "  "); err != nil {
 			return errors.WithStack(err)
-		}
-		err = path.WriteFile(bytes)
-		if err != nil {
+		} else if err := cache.cacheFilePath.WriteFile(bytes); err != nil {
 			return errors.WithStack(err)
 		}
 	}
 	return nil
 }
 
-func findIncludesUntilDone(ctx *types.Context, cache *includeCache, sourceFile types.SourceFile) error {
-	sourcePath := sourceFile.SourcePath(ctx)
+func (cache *includeCache) String() string {
+	res := "[\n"
+	for _, entry := range cache.entries {
+		res = res + "  " + entry.String() + "\n"
+	}
+	return res + "]"
+}
+
+func (f *CppIncludesFinder) findIncludesUntilDone(sourceFile *SourceFile) error {
+	sourcePath := sourceFile.SourcePath()
 	targetFilePath := paths.NullPath()
-	depPath := sourceFile.DepfilePath(ctx)
-	objPath := sourceFile.ObjectPath(ctx)
+	depPath := sourceFile.DepfilePath()
+	objPath := sourceFile.ObjectPath()
 
 	// TODO: This should perhaps also compare against the
 	// include.cache file timestamp. Now, it only checks if the file
@@ -312,75 +384,58 @@ func findIncludesUntilDone(ctx *types.Context, cache *includeCache, sourceFile t
 	// TODO: This reads the dependency file, but the actual building
 	// does it again. Should the result be somehow cached? Perhaps
 	// remove the object file if it is found to be stale?
-	unchanged, err := builder_utils.ObjFileIsUpToDate(ctx, sourcePath, objPath, depPath)
+	unchanged, err := builder_utils.ObjFileIsUpToDate(f.ctx, sourcePath, objPath, depPath)
 	if err != nil {
 		return errors.WithStack(err)
 	}
 
 	first := true
 	for {
-		var include string
-		cache.ExpectFile(sourcePath)
+		f.cache.ExpectFile(sourcePath)
 
-		includes := ctx.IncludeFolders
-		if library, ok := sourceFile.Origin.(*libraries.Library); ok && library.UtilityDir != nil {
-			includes = append(includes, library.UtilityDir)
-		}
-
-		if library, ok := sourceFile.Origin.(*libraries.Library); ok {
-			if library.Precompiled && library.PrecompiledWithSources {
-				// Fully precompiled libraries should have no dependencies
-				// to avoid ABI breakage
-				if ctx.Verbose {
-					ctx.Info(tr("Skipping dependencies detection for precompiled library %[1]s", library.Name))
-				}
-				return nil
-			}
-		}
-
-		var preproc_err error
-		var preproc_stderr []byte
-
-		if unchanged && cache.valid {
-			include = cache.Next().Include
-			if first && ctx.Verbose {
-				ctx.Info(tr("Using cached library dependencies for file: %[1]s", sourcePath))
+		var preprocErr error
+		var preprocStderr []byte
+		var include string
+		if unchanged && f.cache.valid {
+			include = f.cache.Peek().Include
+			if first && f.ctx.Verbose {
+				f.ctx.Info(tr("Using cached library dependencies for file: %[1]s", sourcePath))
 			}
 		} else {
-			preproc_stderr, preproc_err = GCCPreprocRunnerForDiscoveringIncludes(ctx, sourcePath, targetFilePath, includes)
+			preprocStderr, preprocErr = GCCPreprocRunnerForDiscoveringIncludes(f.ctx, sourcePath, targetFilePath, f.IncludeDirsFound)
 			// Unwrap error and see if it is an ExitError.
-			_, is_exit_error := errors.Cause(preproc_err).(*exec.ExitError)
-			if preproc_err == nil {
+			_, isExitError := errors.Cause(preprocErr).(*exec.ExitError)
+			if preprocErr == nil {
 				// Preprocessor successful, done
 				include = ""
-			} else if !is_exit_error || preproc_stderr == nil {
+			} else if !isExitError || preprocStderr == nil {
 				// Ignore ExitErrors (e.g. gcc returning
 				// non-zero status), but bail out on
 				// other errors
-				return errors.WithStack(preproc_err)
+				return errors.WithStack(preprocErr)
 			} else {
-				include = IncludesFinderWithRegExp(string(preproc_stderr))
-				if include == "" && ctx.Verbose {
-					ctx.Info(tr("Error while detecting libraries included by %[1]s", sourcePath))
+				include = IncludesFinderWithRegExp(string(preprocStderr))
+				if include == "" && f.ctx.Verbose {
+					f.ctx.Info(tr("Error while detecting libraries included by %[1]s", sourcePath))
 				}
 			}
 		}
 
 		if include == "" {
 			// No missing includes found, we're done
-			cache.ExpectEntry(sourcePath, "", nil)
+			f.cache.AddAndCheckEntry(sourcePath, "", nil)
 			return nil
 		}
 
-		library := ResolveLibrary(ctx, include)
+		library := ResolveLibrary(f.ctx, include)
 		if library == nil {
 			// Library could not be resolved, show error
 			// err := runCommand(ctx, &GCCPreprocRunner{SourceFilePath: sourcePath, TargetFileName: paths.New(constants.FILE_CTAGS_TARGET_FOR_GCC_MINUS_E), Includes: includes})
 			// return errors.WithStack(err)
-			if preproc_err == nil || preproc_stderr == nil {
+			if preprocErr == nil || preprocStderr == nil {
 				// Filename came from cache, so run preprocessor to obtain error to show
-				preproc_stderr, preproc_err = GCCPreprocRunnerForDiscoveringIncludes(ctx, sourcePath, targetFilePath, includes)
-				if preproc_err == nil {
+				preprocStderr, preprocErr = GCCPreprocRunnerForDiscoveringIncludes(f.ctx, sourcePath, targetFilePath, f.IncludeDirsFound)
+				if preprocErr == nil {
 					// If there is a missing #include in the cache, but running
 					// gcc does not reproduce that, there is something wrong.
 					// Returning an error here will cause the cache to be
@@ -388,39 +443,130 @@ func findIncludesUntilDone(ctx *types.Context, cache *includeCache, sourceFile t
 					return errors.New(tr("Internal error in cache"))
 				}
 			}
-			ctx.Stderr.Write(preproc_stderr)
-			return errors.WithStack(preproc_err)
+			f.ctx.Stderr.Write(preprocStderr)
+			return errors.WithStack(preprocErr)
 		}
 
-		// Add this library to the list of libraries, the
-		// include path and queue its source files for further
-		// include scanning
-		ctx.ImportedLibraries = append(ctx.ImportedLibraries, library)
-		appendIncludeFolder(ctx, cache, sourcePath, include, library.SourceDir)
+		// Add this library to the list of libraries found, add the include path
+		// and queue its source files for further include scanning
+		f.LibrariesFound.Add(library)
+		f.cache.AddAndCheckEntry(sourcePath, include, library.SourceDir)
+
 		sourceDirs := library.SourceDirs()
+		buildDir := f.ctx.LibrariesBuildPath.Join(library.Name)
 		for _, sourceDir := range sourceDirs {
-			queueSourceFilesFromFolder(ctx, ctx.CollectedSourceFiles, library, sourceDir.Dir, sourceDir.Recurse)
+			f.log.Debugf("Using library include folder: %s", sourceDir.Dir)
+			f.IncludeDirsFound.Add(sourceDir.Dir)
+			if library.Precompiled && library.PrecompiledWithSources {
+				// Fully precompiled libraries should have no dependencies
+				// to avoid ABI breakage
+				if f.ctx.Verbose {
+					f.ctx.Info(tr("Skipping dependencies detection for precompiled library %[1]s", library.Name))
+				}
+			} else {
+				f.AddSourceDir(buildDir, sourceDir.Dir, sourceDir.Recurse)
+			}
 		}
 		first = false
 	}
 }
 
-func queueSourceFilesFromFolder(ctx *types.Context, queue *types.UniqueSourceFileQueue, origin interface{}, folder *paths.Path, recurse bool) error {
-	extensions := func(ext string) bool { return ADDITIONAL_FILE_VALID_EXTENSIONS_NO_HEADERS[ext] }
+// SourceFile represent a source file, it keeps a reference to the root source
+// directory and the corresponding build root directory.
+type SourceFile struct {
+	// SourceRoot is the path to the source code root directory
+	SourceRoot *paths.Path
 
-	filePaths := []string{}
-	err := utils.FindFilesInFolder(&filePaths, folder.String(), extensions, recurse)
-	if err != nil {
-		return errors.WithStack(err)
-	}
+	// BuildRoot is the path to the build root directory
+	BuildRoot *paths.Path
 
-	for _, filePath := range filePaths {
-		sourceFile, err := types.MakeSourceFile(ctx, origin, paths.New(filePath))
-		if err != nil {
-			return errors.WithStack(err)
+	// Path to the source file within the sketch/library root folder
+	RelativePath *paths.Path
+}
+
+func (f *SourceFile) String() string {
+	return fmt.Sprintf("Root: %s - Path: %s - BuildPath: %s",
+		f.SourceRoot, f.RelativePath, f.BuildRoot)
+}
+
+// MakeSourceFile creates a SourceFile containing the given source file path
+// within the given sourceRoot. If srcPath is absolute it is transformed to
+// relative to sourceRoot.
+func MakeSourceFile(sourceRoot, buildRoot, srcPath *paths.Path) (*SourceFile, error) {
+	if srcPath.IsAbs() {
+		if relPath, err := sourceRoot.RelTo(srcPath); err == nil {
+			srcPath = relPath
+		} else {
+			return nil, err
 		}
-		queue.Push(sourceFile)
 	}
+	return &SourceFile{SourceRoot: sourceRoot, BuildRoot: buildRoot, RelativePath: srcPath}, nil
+}
 
-	return nil
+// SourcePath returns the path to the source file
+func (f *SourceFile) SourcePath() *paths.Path {
+	return f.SourceRoot.JoinPath(f.RelativePath)
+}
+
+// ObjectPath returns the path to the object file (.o)
+func (f *SourceFile) ObjectPath() *paths.Path {
+	return f.BuildRoot.Join(f.RelativePath.String() + ".o")
+}
+
+// DepfilePath returns the path to the dependencies file (.d)
+func (f *SourceFile) DepfilePath() *paths.Path {
+	return f.BuildRoot.Join(f.RelativePath.String() + ".d")
+}
+
+// Equals return true if the SourceFile equals to the SourceFile
+// passed as parameter
+func (f *SourceFile) Equals(other *SourceFile) bool {
+	return f.BuildRoot.EqualsTo(other.BuildRoot) &&
+		f.SourceRoot.EqualsTo(other.SourceRoot) &&
+		f.RelativePath.EqualsTo(other.RelativePath)
+}
+
+// UniqueSourceFileQueue is a queue of SourceFile. A SourceFile
+// can be pushed in the queue only once.
+type UniqueSourceFileQueue struct {
+	queue []*SourceFile
+	curr  int
+}
+
+// Len returns the number of element waiting in the queue
+func (q *UniqueSourceFileQueue) Len() int {
+	return len(q.queue) - q.curr
+}
+
+// Push insert a new element in the queue
+func (q *UniqueSourceFileQueue) Push(value *SourceFile) {
+	if !q.Contains(value) {
+		q.queue = append(q.queue, value)
+	}
+}
+
+// Pop return the first element in the queue or nil if the queue is empty
+func (q *UniqueSourceFileQueue) Pop() *SourceFile {
+	if q.Empty() {
+		return nil
+	}
+	res := q.queue[q.curr]
+	q.curr++
+	return res
+}
+
+// Empty returns true if the queue is empty
+func (q *UniqueSourceFileQueue) Empty() bool {
+	return q.Len() == 0
+}
+
+// Contains return true if the target elemen has been already added
+// in the queue (even if the element has been alread popped out)
+func (q *UniqueSourceFileQueue) Contains(target *SourceFile) bool {
+	for _, elem := range q.queue {
+		if elem.Equals(target) {
+			return true
+		}
+	}
+	return false
 }
diff --git a/legacy/builder/create_cmake_rule.go b/legacy/builder/create_cmake_rule.go
index 9e8f499605c..bd1e588839c 100644
--- a/legacy/builder/create_cmake_rule.go
+++ b/legacy/builder/create_cmake_rule.go
@@ -89,8 +89,7 @@ func (s *ExportProjectCMake) Run(ctx *types.Context) error {
 		}
 
 		// Remove stray folders contining incompatible or not needed libraries archives
-		var files []string
-		utils.FindFilesInFolder(&files, libDir.Join("src").String(), staticLibsExtensions, true)
+		files, _ := utils.FindFilesInFolder(libDir.Join("src").String(), staticLibsExtensions, true)
 		for _, file := range files {
 			staticLibDir := filepath.Dir(file)
 			if !isStaticLib || !strings.Contains(staticLibDir, mcu) {
@@ -126,8 +125,7 @@ func (s *ExportProjectCMake) Run(ctx *types.Context) error {
 	}
 
 	// remove "#line 1 ..." from exported c_make folder sketch
-	var sketchFiles []string
-	utils.FindFilesInFolder(&sketchFiles, cmakeFolder.Join("sketch").String(), extensions, false)
+	sketchFiles, _ := utils.FindFilesInFolder(cmakeFolder.Join("sketch").String(), extensions, false)
 
 	for _, file := range sketchFiles {
 		input, err := ioutil.ReadFile(file)
@@ -161,14 +159,12 @@ func (s *ExportProjectCMake) Run(ctx *types.Context) error {
 	extractCompileFlags(ctx, constants.RECIPE_CPP_PATTERN, &defines, &dynamicLibsFromGccMinusL, &linkerflags, &linkDirectories)
 
 	// Extract folders with .h in them for adding in include list
-	var headerFiles []string
 	isHeader := func(ext string) bool { return DOTHEXTENSION[ext] }
-	utils.FindFilesInFolder(&headerFiles, cmakeFolder.String(), isHeader, true)
+	headerFiles, _ := utils.FindFilesInFolder(cmakeFolder.String(), isHeader, true)
 	foldersContainingDotH := findUniqueFoldersRelative(headerFiles, cmakeFolder.String())
 
 	// Extract folders with .a in them for adding in static libs paths list
-	var staticLibs []string
-	utils.FindFilesInFolder(&staticLibs, cmakeFolder.String(), staticLibsExtensions, true)
+	staticLibs, _ := utils.FindFilesInFolder(cmakeFolder.String(), staticLibsExtensions, true)
 
 	// Generate the CMakeLists global file
 
diff --git a/legacy/builder/test/add_additional_entries_to_context_test.go b/legacy/builder/test/add_additional_entries_to_context_test.go
index ab64c26ec4c..51e5452daf3 100644
--- a/legacy/builder/test/add_additional_entries_to_context_test.go
+++ b/legacy/builder/test/add_additional_entries_to_context_test.go
@@ -38,8 +38,6 @@ func TestAddAdditionalEntriesToContextNoBuildPath(t *testing.T) {
 
 	require.NotNil(t, ctx.WarningsLevel)
 
-	require.True(t, ctx.CollectedSourceFiles.Empty())
-
 	require.Equal(t, 0, len(ctx.LibrariesResolutionResults))
 }
 
@@ -57,7 +55,5 @@ func TestAddAdditionalEntriesToContextWithBuildPath(t *testing.T) {
 
 	require.NotNil(t, ctx.WarningsLevel)
 
-	require.True(t, ctx.CollectedSourceFiles.Empty())
-
 	require.Equal(t, 0, len(ctx.LibrariesResolutionResults))
 }
diff --git a/legacy/builder/test/includes_to_include_folders_test.go b/legacy/builder/test/includes_to_include_folders_test.go
index 86836bd5c49..8fc5878e82f 100644
--- a/legacy/builder/test/includes_to_include_folders_test.go
+++ b/legacy/builder/test/includes_to_include_folders_test.go
@@ -23,6 +23,7 @@ import (
 	"github.com/arduino/arduino-cli/legacy/builder"
 	"github.com/arduino/arduino-cli/legacy/builder/types"
 	paths "github.com/arduino/go-paths-helper"
+	"github.com/sirupsen/logrus"
 	"github.com/stretchr/testify/require"
 )
 
@@ -297,6 +298,7 @@ func TestIncludesToIncludeFoldersSubfolders(t *testing.T) {
 		Verbose:              true,
 	}
 
+	logrus.SetLevel(logrus.DebugLevel)
 	buildPath := SetupBuildPath(t, ctx)
 	defer buildPath.RemoveAll()
 
diff --git a/legacy/builder/test/try_build_of_problematic_sketch_test.go b/legacy/builder/test/try_build_of_problematic_sketch_test.go
index 92c5afb1f3f..8fa368fc527 100644
--- a/legacy/builder/test/try_build_of_problematic_sketch_test.go
+++ b/legacy/builder/test/try_build_of_problematic_sketch_test.go
@@ -17,12 +17,15 @@
 package test
 
 import (
+	"fmt"
 	"path/filepath"
 	"testing"
+	"time"
 
 	"github.com/arduino/arduino-cli/legacy/builder"
 	"github.com/arduino/arduino-cli/legacy/builder/types"
 	paths "github.com/arduino/go-paths-helper"
+	"github.com/stretchr/testify/require"
 )
 
 func TestTryBuild001(t *testing.T) {
@@ -225,7 +228,21 @@ func makeDefaultContext(t *testing.T) *types.Context {
 
 func tryBuild(t *testing.T, sketchPath ...string) {
 	ctx := makeDefaultContext(t)
+	ctx.DebugLevel = 999
+	s1 := time.Now()
 	tryBuildWithContext(t, ctx, sketchPath...)
+	t1 := time.Now().Sub(s1)
+
+	ctx2 := makeDefaultContext(t)
+	ctx2.DebugLevel = 999
+	ctx2.BuildPath = ctx.BuildPath
+	s2 := time.Now()
+	tryBuildWithContext(t, ctx2, sketchPath...)
+	t2 := time.Now().Sub(s2)
+
+	fmt.Println("Build time non-cached:", t1)
+	fmt.Println("Build time cached:", t2)
+	require.True(t, t1.Microseconds() > t2.Microseconds()*9/10, "cached build time should be much lower")
 }
 
 func tryBuildWithContext(t *testing.T, ctx *types.Context, sketchPath ...string) {
diff --git a/legacy/builder/types/accessories.go b/legacy/builder/types/accessories.go
index b0c85d37e20..c178c15c61e 100644
--- a/legacy/builder/types/accessories.go
+++ b/legacy/builder/types/accessories.go
@@ -43,29 +43,6 @@ func (queue *UniqueStringQueue) Empty() bool {
 	return queue.Len() == 0
 }
 
-type UniqueSourceFileQueue []SourceFile
-
-func (queue UniqueSourceFileQueue) Len() int           { return len(queue) }
-func (queue UniqueSourceFileQueue) Less(i, j int) bool { return false }
-func (queue UniqueSourceFileQueue) Swap(i, j int)      { panic("Who called me?!?") }
-
-func (queue *UniqueSourceFileQueue) Push(value SourceFile) {
-	if !sliceContainsSourceFile(*queue, value) {
-		*queue = append(*queue, value)
-	}
-}
-
-func (queue *UniqueSourceFileQueue) Pop() SourceFile {
-	old := *queue
-	x := old[0]
-	*queue = old[1:]
-	return x
-}
-
-func (queue *UniqueSourceFileQueue) Empty() bool {
-	return queue.Len() == 0
-}
-
 type BufferedUntilNewLineWriter struct {
 	PrintFunc PrintFunc
 	Buffer    bytes.Buffer
diff --git a/legacy/builder/types/context.go b/legacy/builder/types/context.go
index 77610afbb8e..503ee48594d 100644
--- a/legacy/builder/types/context.go
+++ b/legacy/builder/types/context.go
@@ -111,8 +111,6 @@ type Context struct {
 	SketchObjectFiles            paths.PathList
 	IgnoreSketchFolderNameErrors bool
 
-	CollectedSourceFiles *UniqueSourceFileQueue
-
 	Sketch          *sketch.Sketch
 	Source          string
 	SourceGccMinusE string
diff --git a/legacy/builder/types/types.go b/legacy/builder/types/types.go
index 70ee1c34608..fbb7dc2d58e 100644
--- a/legacy/builder/types/types.go
+++ b/legacy/builder/types/types.go
@@ -16,75 +16,12 @@
 package types
 
 import (
-	"fmt"
 	"strconv"
 
 	"github.com/arduino/arduino-cli/arduino/libraries"
-	"github.com/arduino/arduino-cli/arduino/sketch"
 	paths "github.com/arduino/go-paths-helper"
 )
 
-type SourceFile struct {
-	// Sketch or Library pointer that this source file lives in
-	Origin interface{}
-	// Path to the source file within the sketch/library root folder
-	RelativePath *paths.Path
-}
-
-// Create a SourceFile containing the given source file path within the
-// given origin. The given path can be absolute, or relative within the
-// origin's root source folder
-func MakeSourceFile(ctx *Context, origin interface{}, path *paths.Path) (SourceFile, error) {
-	if path.IsAbs() {
-		var err error
-		path, err = sourceRoot(ctx, origin).RelTo(path)
-		if err != nil {
-			return SourceFile{}, err
-		}
-	}
-	return SourceFile{Origin: origin, RelativePath: path}, nil
-}
-
-// Return the build root for the given origin, where build products will
-// be placed. Any directories inside SourceFile.RelativePath will be
-// appended here.
-func buildRoot(ctx *Context, origin interface{}) *paths.Path {
-	switch o := origin.(type) {
-	case *sketch.Sketch:
-		return ctx.SketchBuildPath
-	case *libraries.Library:
-		return ctx.LibrariesBuildPath.Join(o.Name)
-	default:
-		panic("Unexpected origin for SourceFile: " + fmt.Sprint(origin))
-	}
-}
-
-// Return the source root for the given origin, where its source files
-// can be found. Prepending this to SourceFile.RelativePath will give
-// the full path to that source file.
-func sourceRoot(ctx *Context, origin interface{}) *paths.Path {
-	switch o := origin.(type) {
-	case *sketch.Sketch:
-		return ctx.SketchBuildPath
-	case *libraries.Library:
-		return o.SourceDir
-	default:
-		panic("Unexpected origin for SourceFile: " + fmt.Sprint(origin))
-	}
-}
-
-func (f *SourceFile) SourcePath(ctx *Context) *paths.Path {
-	return sourceRoot(ctx, f.Origin).JoinPath(f.RelativePath)
-}
-
-func (f *SourceFile) ObjectPath(ctx *Context) *paths.Path {
-	return buildRoot(ctx, f.Origin).Join(f.RelativePath.String() + ".o")
-}
-
-func (f *SourceFile) DepfilePath(ctx *Context) *paths.Path {
-	return buildRoot(ctx, f.Origin).Join(f.RelativePath.String() + ".d")
-}
-
 type SketchFile struct {
 	Name *paths.Path
 }
diff --git a/legacy/builder/types/utils.go b/legacy/builder/types/utils.go
index 7102cd555ca..3b514831518 100644
--- a/legacy/builder/types/utils.go
+++ b/legacy/builder/types/utils.go
@@ -24,12 +24,3 @@ func sliceContains(slice []string, target string) bool {
 	}
 	return false
 }
-
-func sliceContainsSourceFile(slice []SourceFile, target SourceFile) bool {
-	for _, elem := range slice {
-		if elem.Origin == target.Origin && elem.RelativePath.EqualsTo(target.RelativePath) {
-			return true
-		}
-	}
-	return false
-}
diff --git a/legacy/builder/utils/utils.go b/legacy/builder/utils/utils.go
index 2f60ff75719..da9653985f5 100644
--- a/legacy/builder/utils/utils.go
+++ b/legacy/builder/utils/utils.go
@@ -259,7 +259,8 @@ func FindAllSubdirectories(folder string, output *[]string) error {
 	return gohasissues.Walk(folder, walkFunc)
 }
 
-func FindFilesInFolder(files *[]string, folder string, extensions CheckExtensionFunc, recurse bool) error {
+func FindFilesInFolder(folder string, extensions CheckExtensionFunc, recurse bool) ([]string, error) {
+	files := []string{}
 	walkFunc := func(path string, info os.FileInfo, err error) error {
 		if err != nil {
 			return err
@@ -295,10 +296,11 @@ func FindFilesInFolder(files *[]string, folder string, extensions CheckExtension
 		}
 		currentFile.Close()
 
-		*files = append(*files, path)
+		files = append(files, path)
 		return nil
 	}
-	return gohasissues.Walk(folder, walkFunc)
+	err := gohasissues.Walk(folder, walkFunc)
+	return files, err
 }
 
 func AppendIfNotPresent(target []string, elements ...string) []string {